public void drawSphere(float radius,ref btTransform transform,ref btVector3 color)
        {
            btVector3 start = transform.Origin;

            btVector3 xoffs;// = btMatrix3x3.Multiply(transform.Basis, new btVector3(radius, 0, 0));
            btVector3 yoffs;// = btMatrix3x3.Multiply(transform.Basis, new btVector3(0, radius, 0));
            btVector3 zoffs;// = btMatrix3x3.Multiply(transform.Basis, new btVector3(0, 0, radius));
            {
                btVector3 temp;
                temp = new btVector3(radius, 0, 0);
                btMatrix3x3.Multiply(ref transform.Basis, ref temp, out xoffs);
                temp = new btVector3(0, radius, 0);
                btMatrix3x3.Multiply(ref transform.Basis, ref temp, out yoffs);
                temp = new btVector3(0, 0, radius);
                btMatrix3x3.Multiply(ref transform.Basis, ref temp, out zoffs);
            }
            // XY 
            /*drawLine(start - xoffs, start + yoffs, color);
            drawLine(start + yoffs, start + xoffs, color);
            drawLine(start + xoffs, start - yoffs, color);
            drawLine(start - yoffs, start - xoffs, color);
            

            // XZ
            drawLine(start - xoffs, start + zoffs, color);
            drawLine(start + zoffs, start + xoffs, color);
            drawLine(start + xoffs, start - zoffs, color);
            drawLine(start - zoffs, start - xoffs, color);

            // YZ
            drawLine(start - yoffs, start + zoffs, color);
            drawLine(start + zoffs, start + yoffs, color);
            drawLine(start + yoffs, start - zoffs, color);
            drawLine(start - zoffs, start - yoffs, color);*/
            btVector3 sax, say, saz, smx, smy, smz;
            btVector3.Add(ref start, ref xoffs, out sax);
            btVector3.Add(ref start, ref yoffs, out say);
            btVector3.Add(ref start, ref zoffs, out saz);
            btVector3.Subtract(ref start, ref xoffs, out smx);
            btVector3.Subtract(ref start, ref yoffs, out smy);
            btVector3.Subtract(ref start, ref zoffs, out smz);
            // XY 
            drawLine(ref smx, ref say, ref color);
            drawLine(ref say, ref sax, ref color);
            drawLine(ref sax, ref smy, ref color);
            drawLine(ref smy, ref smx, ref color);


            // XZ
            drawLine(ref smx, ref saz, ref color);
            drawLine(ref saz, ref sax, ref color);
            drawLine(ref sax, ref smz, ref color);
            drawLine(ref smz, ref smx, ref color);

            // YZ
            drawLine(ref smy, ref saz, ref color);
            drawLine(ref saz, ref say, ref color);
            drawLine(ref say, ref smz, ref color);
            drawLine(ref smz, ref smy, ref color);
        }

        public bool calcPenDepth(ISimplexSolver simplexSolver, ConvexShape pConvexA, ConvexShape pConvexB, btTransform transformA, btTransform transformB, ref btVector3 v, out btVector3 wWitnessOnA, out btVector3 wWitnessOnB, IDebugDraw debugDraw)
        {
            btVector3 guessVector;// = transformA.Origin - transformB.Origin;
            btVector3.Subtract(ref transformA.Origin,ref transformB.Origin, out guessVector);
            GjkEpaSolver2.sResults results = new GjkEpaSolver2.sResults();

            
            if (GjkEpaSolver2.Penetration(pConvexA, transformA,
                                        pConvexB, transformB,
                                        guessVector, ref results))
            {
                //	debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
                //resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
                wWitnessOnA = results.witnesses0;
                wWitnessOnB = results.witnesses1;
                v = results.normal;
                return true;
            }
            else
            {
                if (GjkEpaSolver2.Distance(pConvexA, transformA, pConvexB, transformB, guessVector, ref results))
                {
                    wWitnessOnA = results.witnesses0;
                    wWitnessOnB = results.witnesses1;
                    v = results.normal;
                    return false;
                }
            }
            wWitnessOnA = new btVector3();
            wWitnessOnB = new btVector3();
            return false;
            
        }

        public BroadphaseProxy createProxy(ref btVector3 aabbMin,ref btVector3 aabbMax,
            BroadphaseNativeTypes shapeType, object userPtr,
            short collisionFilterGroup, short collisionFilterMask, IDispatcher dispatcher,
            object multiSapProxy)
        {

            DbvtProxy proxy = new DbvtProxy(aabbMin, aabbMax, userPtr,
                collisionFilterGroup,
                collisionFilterMask);

            DbvtAabbMm aabb;// = DbvtAabbMm.FromMM(aabbMin, aabbMax);
            DbvtAabbMm.FromMM(ref aabbMin, ref aabbMax, out aabb);

            //bproxy->aabb			=	btDbvtVolume::FromMM(aabbMin,aabbMax);
            proxy.stage = m_stageCurrent;
            proxy.m_uniqueId = ++m_gid;
            proxy.leaf = m_sets[0].insert(ref aabb, proxy);
            listappend(ref proxy, ref m_stageRoots[m_stageCurrent]);
            if (!m_deferedcollide)
            {
                DbvtTreeCollider collider = new DbvtTreeCollider(this);
                collider.proxy = proxy;
                m_sets[0].collideTV(m_sets[0].m_root, ref aabb, collider);
                m_sets[1].collideTV(m_sets[1].m_root,ref aabb, collider);
            }
            return (proxy);
        }

 public void Support(ref btVector3 d, uint index,out btVector3 result)
 {
     if (index != 0)
         Support1(ref d,out result);
     else
         Support0(ref d,out result);
 }

        //brute force implementations
        public override void localGetSupportingVertex(ref btVector3 vec0, out btVector3 supVec)
        {
            supVec = btVector3.Zero;
            int i;
            float maxDot = -BulletGlobal.BT_LARGE_FLOAT;

            btVector3 vec = vec0;
            float lenSqr = vec.Length2;
            if (lenSqr < 0.0001f)
            {
                vec.setValue(1, 0, 0);
            }
            else
            {
                float rlen = 1f / (float)Math.Sqrt(lenSqr);
                vec *= rlen;
            }

            btVector3 vtx;
            float newDot;

            for (i = 0; i < NumVertices; i++)
            {
                getVertex(i, out vtx);
                newDot = vec.dot(vtx);
                if (newDot > maxDot)
                {
                    maxDot = newDot;
                    supVec = vtx;
                }
            }


            //return supVec;
        }

 public static void Add(ref btVector3 v1, ref btVector3 v2, out btVector3 result)
 {
     result.X = v1.X + v2.X;
     result.Y = v1.Y + v2.Y;
     result.Z = v1.Z + v2.Z;
     result.W = 0;
 }

        public void updateSingleAabb(CollisionObject colObj)
        {
            btVector3 minAabb, maxAabb;
            colObj.CollisionShape.getAabb(colObj.WorldTransform, out minAabb, out maxAabb);
            //need to increase the aabb for contact thresholds
            btVector3 contactThreshold = new btVector3(PersistentManifold.gContactBreakingThreshold, PersistentManifold.gContactBreakingThreshold, PersistentManifold.gContactBreakingThreshold);
            //minAabb -= contactThreshold;
            //maxAabb += contactThreshold;
            minAabb.Subtract(ref contactThreshold);
            maxAabb.Add(ref contactThreshold);

            //IBroadphaseInterface bp = m_broadphasePairCache;

            //moving objects should be moderately sized, probably something wrong if not
            if (colObj.isStaticObject || ((maxAabb - minAabb).Length2 < 1e12f))
            {
                m_broadphasePairCache.setAabb(colObj.BroadphaseHandle,ref minAabb,ref maxAabb, m_dispatcher1);
            }
            else
            {
                //something went wrong, investigate
                //this assert is unwanted in 3D modelers (danger of loosing work)
                colObj.ActivationState = ActivationStateFlags.DISABLE_SIMULATION;

                if (reportMe && m_debugDrawer != null)
                {
                    reportMe = false;
                    m_debugDrawer.reportErrorWarning("Overflow in AABB, object removed from simulation");
                    m_debugDrawer.reportErrorWarning("If you can reproduce this, please email [email protected]\n");
                    m_debugDrawer.reportErrorWarning("Please include above information, your Platform, version of OS.\n");
                    m_debugDrawer.reportErrorWarning("Thanks.\n");
                }
            }
        }

        public override void batchedUnitVectorGetSupportingVertexWithoutMargin(btVector3[] vectors, btVector3[] supportVerticesOut, int numVectors)
        {
            int i;

            btVector3 vtx;
            float newDot;

            for (i = 0; i < numVectors; i++)
            {
                supportVerticesOut[i].W = -BulletGlobal.BT_LARGE_FLOAT;
            }

            for (int j = 0; j < numVectors; j++)
            {

                btVector3 vec = vectors[j];

                for (i = 0; i < NumVertices; i++)
                {
                    getVertex(i, out vtx);
                    newDot = vec.dot(vtx);
                    if (newDot > supportVerticesOut[j].W)
                    {
                        //WARNING: don't swap next lines, the w component would get overwritten!
                        supportVerticesOut[j] = vtx;
                        supportVerticesOut[j].W = newDot;
                    }
                }
            }
        }

 public static void Multiply(ref btVector3 v, ref btMatrix3x3 m, out btVector3 result)
 {
     result.X = m.tdotx(ref v);
     result.Y = m.tdoty(ref v);
     result.Z = m.tdotz(ref v);
     result.W = 0;
 }

        public static void	calculateVelocity(btTransform transform0,btTransform transform1,float timeStep,out btVector3 linVel, out btVector3 angVel)
	    {
		    linVel = (transform1.Origin - transform0.Origin) / timeStep;
		    btVector3 axis;
		    float  angle;
		    calculateDiffAxisAngle(transform0,transform1,out axis,out angle);
		    angVel = axis * angle / timeStep;
	    }

 public void reset()
 {
     m_cachedValidClosest = false;
     m_numVertices = 0;
     m_needsUpdate = true;
     m_lastW = new btVector3(BulletGlobal.BT_LARGE_FLOAT, BulletGlobal.BT_LARGE_FLOAT, BulletGlobal.BT_LARGE_FLOAT);
     m_cachedBC.reset();
 }

        public override void localGetSupportingVertexWithoutMargin(ref btVector3 vec,out btVector3 result)
        {
            btVector3 halfExtents = HalfExtentsWithoutMargin;

            result= new btVector3((vec.X >= 0 ? halfExtents.X : -halfExtents.X),
                (vec.Y >= 0 ? halfExtents.Y : -halfExtents.Y),
                (vec.Z >= 0 ? halfExtents.Z : -halfExtents.Z));
        }

 /// <summary>
 /// 線の描画
 /// </summary>
 /// <param name="from">開始点</param>
 /// <param name="to">終了点</param>
 /// <param name="color">色</param>
 /// <remarks>スーパークラスから呼び出される</remarks>
 public override void drawLine(ref btVector3 from, ref btVector3 to, ref btVector3 color)
 {
     lines[primitiveCount * 2].pos = new Vector3(from.X, from.Y, from.Z);
     lines[primitiveCount * 2].color = ((int)(color.X*256)) * (2 ^ 16) + ((int)(color.Y*256)) * (2 ^ 8) + ((int)(color.Z*256));
     lines[primitiveCount * 2 + 1].pos = new Vector3(to.X, to.Y, to.Z);
     lines[primitiveCount * 2 + 1].color = ((int)(color.X * 256)) * (2 ^ 16) + ((int)(color.Y * 256)) * (2 ^ 8) + ((int)(color.Z * 256));
     ++primitiveCount;
 }

        public btMatrix3x3(float xx, float xy, float xz,
		    float yx, float yy, float yz,
		    float zx, float zy, float zz)
	    {
            el0=new btVector3(xx, xy, xz);
            el1 = new btVector3(yx, yy, yz);
            el2 = new btVector3(zx, zy, zz);
        }

 public BroadphaseProxy(btVector3 aabbMin, btVector3 aabbMax, object userPtr, short collisionFilterGroup, short collisionFilterMask)
 {
     m_clientObject = userPtr;
     m_collisionFilterGroup = collisionFilterGroup;
     m_collisionFilterMask = collisionFilterMask;
     m_aabbMin = aabbMin;
     m_aabbMax = aabbMax;
     m_multiSapParentProxy = null;
 }

        public override void processCollision(CollisionObject col0, CollisionObject col1, DispatcherInfo dispatchInfo, ref ManifoldResult resultOut)
        {
            if (m_manifoldPtr == null)
                return;

            resultOut.PersistentManifold = m_manifoldPtr;

            SphereShape sphere0 = (SphereShape)col0.CollisionShape;
            SphereShape sphere1 = (SphereShape)col1.CollisionShape;

            btVector3 diff = col0.WorldTransform.Origin - col1.WorldTransform.Origin;
            float len = diff.Length;
            float radius0 = sphere0.Radius;
            float radius1 = sphere1.Radius;

#if CLEAR_MANIFOLD
	        m_manifoldPtr->clearManifold(); //don't do this, it disables warmstarting
#endif

            ///iff distance positive, don't generate a new contact
            if (len > (radius0 + radius1))
            {
#if! CLEAR_MANIFOLD
                resultOut.refreshContactPoints();
#endif //CLEAR_MANIFOLD
                return;
            }
            ///distance (negative means penetration)
            float dist = len - (radius0 + radius1);

            btVector3 normalOnSurfaceB = new btVector3(1, 0, 0);
            if (len > BulletGlobal.SIMD_EPSILON)
            {
                //normalOnSurfaceB = diff / len;
                btVector3.Divide(ref diff, len, out normalOnSurfaceB);
            }

            ///point on A (worldspace)
            ///btVector3 pos0 = col0->getWorldTransform().getOrigin() - radius0 * normalOnSurfaceB;
            ///point on B (worldspace)
            btVector3 pos1;// = col1.WorldTransform.Origin + radius1 * normalOnSurfaceB;
            {
                btVector3 temp;
                btVector3.Multiply(ref normalOnSurfaceB, radius1, out temp);
                btVector3.Add(ref col1.WorldTransform.Origin, ref temp, out pos1);
            }
            /// report a contact. internally this will be kept persistent, and contact reduction is done


            resultOut.addContactPoint(ref normalOnSurfaceB, ref pos1, dist);

#if! CLEAR_MANIFOLD
            resultOut.refreshContactPoints();
#endif //CLEAR_MANIFOLD

        }

 public btQuaternion(btVector3 axis, float angle)
 {
     float d = axis.Length;
     Debug.Assert(d != 0.0f);
     float s = (float)Math.Sin(angle * 0.5f) / d;
     X = axis.X * s;
     Y = axis.Y * s;
     Z = axis.Z * s;
     W = (float)Math.Cos(angle * 0.5f);
 }

 void Support1(ref btVector3 d,out btVector3 result)
 {
     #region return (m_toshape0 * (m_shapes1).Ls(m_toshape1 * d));
     btVector3 temp,temp2;
     btMatrix3x3.Multiply(ref m_toshape1, ref d, out temp);
     //return (m_toshape0 * (m_shapes1).Ls(temp));
     (m_shapes1).Ls(ref temp, out temp2);
     btTransform.Multiply(ref m_toshape0, ref temp2, out result);
     #endregion
 }

        public StaticPlaneShape(btVector3 planeNormal, float planeConstant)
            : base()
        {
            //m_planeNormal = planeNormal.normalized();
            planeNormal.normalized(out m_planeNormal);
            m_planeConstant = planeConstant;
            m_localScaling = btVector3.Zero;

            //	btAssert( btFuzzyZero(m_planeNormal.length() - btScalar(1.)) );

        }

        public void addVertex(btVector3 w, btVector3 p, btVector3 q)
        {
            m_lastW = w;
            m_needsUpdate = true;

            m_simplexVectorW[m_numVertices] = w;
            m_simplexPointsP[m_numVertices] = p;
            m_simplexPointsQ[m_numVertices] = q;

            m_numVertices++;
        }