public static bool	Distance(ConvexShape shape0,ref IndexedMatrix wtrs0,ConvexShape shape1,ref IndexedMatrix wtrs1,ref IndexedVector3 guess,ref GjkEpaSolver2Results results)
 {
     using (GjkEpaSolver2MinkowskiDiff shape = BulletGlobals.GjkEpaSolver2MinkowskiDiffPool.Get())
     using (GJK gjk = BulletGlobals.GJKPool.Get())
     {
         Initialize(shape0, ref wtrs0, shape1, ref wtrs1, ref results, shape, false);
         gjk.Initialise();
         GJKStatus gjk_status = gjk.Evaluate(shape, ref guess);
         if (gjk_status == GJKStatus.Valid)
         {
             IndexedVector3 w0 = IndexedVector3.Zero;
             IndexedVector3 w1 = IndexedVector3.Zero;
             for (uint i = 0; i < gjk.m_simplex.rank; ++i)
             {
                 float p = gjk.m_simplex.p[i];
                 w0 += shape.Support(ref gjk.m_simplex.c[i].d, 0) * p;
                 IndexedVector3 temp = -gjk.m_simplex.c[i].d;
                 w1 += shape.Support(ref temp, 1) * p;
             }
             results.witnesses0 = wtrs0 * w0;
             results.witnesses1 = wtrs0 * w1;
             results.normal = w0 - w1;
             results.distance = results.normal.Length();
             results.normal /= results.distance > GJK_MIN_DISTANCE ? results.distance : 1;
             return (true);
         }
         else
         {
             //GjkEpaSolver2Status
             results.status = (gjk_status == GJKStatus.Inside) ? GjkEpaSolver2Status.Penetrating : GjkEpaSolver2Status.GJK_Failed;
             return (false);
         }
     }
 }

        public virtual void DrawSphere(float radius, ref IndexedMatrix transform, ref IndexedVector3 color)
        {
            IndexedVector3 start = transform._origin;

            IndexedVector3 xoffs = transform._basis * new IndexedVector3(radius, 0, 0);
            IndexedVector3 yoffs = transform._basis * new IndexedVector3(0, radius, 0);
            IndexedVector3 zoffs = transform._basis * new IndexedVector3(0, 0, radius);

            // 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);
        }

        public override void GetAabb(ref IndexedMatrix t,out IndexedVector3 aabbMin,out IndexedVector3 aabbMax)
        {
            float fmargin = GetMargin();
            IndexedVector3 margin = new IndexedVector3(fmargin);
	        aabbMin = t._origin - margin;
	        aabbMax = t._origin + margin;
        }

 public DefaultMotionState(IndexedMatrix startTrans, IndexedMatrix centerOfMassOffset)
 {
     m_graphicsWorldTrans = startTrans;
     m_startWorldTrans = startTrans;
     m_centerOfMassOffset = centerOfMassOffset;
     m_userPointer = null;
 }

        public GjkEpaPenetrationDepthSolver() { } // for pool

        public virtual bool CalcPenDepth(ISimplexSolverInterface simplexSolver, ConvexShape convexA, ConvexShape convexB, ref IndexedMatrix transA, ref IndexedMatrix transB,
                ref IndexedVector3 v, ref IndexedVector3 wWitnessOnA, ref IndexedVector3 wWitnessOnB, IDebugDraw debugDraw)
        {
            //float radialmargin = 0f;

            IndexedVector3 guessVector = (transA._origin - transB._origin);
            GjkEpaSolver2Results results = new GjkEpaSolver2Results();
            if (GjkEpaSolver2.Penetration(convexA, ref transA,
                                        convexB, ref transB,
                                        ref 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(convexA, ref transA, convexB, ref transB, ref guessVector, ref results))
                {
                    wWitnessOnA = results.witnesses0;
                    wWitnessOnB = results.witnesses1;
                    v = results.normal;
                    return false;
                }
            }
            return false;
        }

		protected virtual int SetAngularLimits(ConstraintInfo2 info, int row_offset, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 linVelA, ref IndexedVector3 linVelB, ref IndexedVector3 angVelA, ref IndexedVector3 angVelB)
		{
			Generic6DofConstraint d6constraint = this;
			int row = row_offset;
			//solve angular limits
			for (int i = 0; i < 3; i++)
			{
				if (d6constraint.GetRotationalLimitMotor(i).NeedApplyTorques())
				{
					IndexedVector3 axis = d6constraint.GetAxis(i);
					int tempFlags = ((int)m_flags) >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);
					SixDofFlags flags = (SixDofFlags)tempFlags;
					if (0 == (flags & SixDofFlags.BT_6DOF_FLAGS_CFM_NORM))
					{
						m_angularLimits[i].m_normalCFM = info.m_solverConstraints[0].m_cfm;
					}
					if (0 == (flags & SixDofFlags.BT_6DOF_FLAGS_CFM_STOP))
					{
						m_angularLimits[i].m_stopCFM = info.m_solverConstraints[0].m_cfm;
					}
					if (0 == (flags & SixDofFlags.BT_6DOF_FLAGS_ERP_STOP))
					{
						m_angularLimits[i].m_stopERP = info.erp;
					}
					row += GetLimitMotorInfo2(d6constraint.GetRotationalLimitMotor(i),
														ref transA, ref transB, ref linVelA, ref linVelB, ref angVelA, ref angVelB, info, row, ref axis, 1, false);
				}
			}

			return row;
		}

public static bool TestInternalObjects( ref IndexedMatrix trans0, ref IndexedMatrix trans1, ref IndexedVector3 delta_c, ref IndexedVector3 axis, ConvexPolyhedron convex0, ConvexPolyhedron convex1, float dmin)
{
	float dp = delta_c.Dot(ref axis);

	IndexedVector3 localAxis0;
	InverseTransformPoint3x3(out localAxis0, ref axis,ref trans0);
	IndexedVector3 localAxis1;
	InverseTransformPoint3x3(out localAxis1, ref axis,ref trans1);

	IndexedVector3 p0;
    BoxSupport(ref convex0.m_extents, ref localAxis0, out p0);
    IndexedVector3 p1;
    BoxSupport(ref convex1.m_extents, ref localAxis1, out p1);

	float Radius0 = p0.X*localAxis0.X + p0.Y*localAxis0.Y + p0.Z*localAxis0.Z;
	float Radius1 = p1.X*localAxis1.X + p1.Y*localAxis1.Y + p1.Z*localAxis1.Z;

	float MinRadius = Radius0>convex0.m_radius ? Radius0 : convex0.m_radius;
	float MaxRadius = Radius1>convex1.m_radius ? Radius1 : convex1.m_radius;

	float MinMaxRadius = MaxRadius + MinRadius;
	float d0 = MinMaxRadius + dp;
	float d1 = MinMaxRadius - dp;

	float depth = d0<d1 ? d0:d1;
    if (depth > dmin)
    {
        return false;
    }
	return true;
}

        public void AddChildShape(ref IndexedMatrix localTransform, CollisionShape shape)
        {
            m_updateRevision++;
            //m_childTransforms.push_back(localTransform);
            //m_childShapes.push_back(shape);
            CompoundShapeChild child = new CompoundShapeChild();
            child.m_transform = localTransform;
            child.m_childShape = shape;
            child.m_childShapeType = shape.GetShapeType();
            child.m_childMargin = shape.GetMargin();

            //extend the local aabbMin/aabbMax
            IndexedVector3 localAabbMin;
            IndexedVector3 localAabbMax;
            shape.GetAabb(ref localTransform, out localAabbMin, out localAabbMax);
            MathUtil.VectorMin(ref localAabbMin, ref m_localAabbMin);
            MathUtil.VectorMax(ref localAabbMax, ref m_localAabbMax);

            if (m_dynamicAabbTree != null)
            {
                DbvtAabbMm bounds = DbvtAabbMm.FromMM(ref localAabbMin, ref localAabbMax);
                int index = m_children.Count;
                child.m_treeNode = m_dynamicAabbTree.Insert(ref bounds, (object)index);
            }

            m_children.Add(child);
        }

        public static void Initialize(ConvexShape shape0,ref IndexedMatrix wtrs0,
            ConvexShape shape1,ref IndexedMatrix wtrs1,
            ref GjkEpaSolver2Results results,
            GjkEpaSolver2MinkowskiDiff shapeR,
            bool withmargins)
        {
            /* Results		*/ 
            results.witnesses0 = IndexedVector3.Zero;
            results.witnesses1 = IndexedVector3.Zero;
            results.status = GjkEpaSolver2Status.Separated;
            /* Shape		*/ 
            shapeR.m_shapes[0] =	shape0;
            shapeR.m_shapes[1] =	shape1;

            shapeR.m_toshape1 = wtrs1._basis.TransposeTimes(ref wtrs0._basis);
            shapeR.m_toshape0 = wtrs0.InverseTimes(ref wtrs1);
#if DEBUG
            if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugGJK)
            {
                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::shape0", shapeR.m_toshape0);
                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::WTRS0", wtrs0);
                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::WTRS1", wtrs1);

                MathUtil.PrintMatrix(BulletGlobals.g_streamWriter, "gjksolver2::init::shape1", shapeR.m_toshape1);
            }
#endif

            shapeR.EnableMargin(withmargins);
        }

 public EntityProperties FromTransform(uint id, IndexedMatrix startTransform)
 {
     EntityProperties ret = new EntityProperties();
     ID = id;
     Position = startTransform._origin;
     Rotation = startTransform.GetRotation();
     return ret;
 }

        public static void GetPlaneEquationTransformed(StaticPlaneShape plane,ref IndexedMatrix trans, out IndexedVector4 equation)
        {
            equation = new IndexedVector4();
            IndexedVector3 planeNormal = plane.GetPlaneNormal();

            equation.X = trans._basis.GetRow(0).Dot(ref planeNormal);
            equation.Y = trans._basis.GetRow(1).Dot(ref planeNormal);
            equation.Z = trans._basis.GetRow(2).Dot(ref planeNormal);
            equation.W = trans._origin.Dot(ref planeNormal) + plane.GetPlaneConstant();
        }

public static void InverseTransformPoint3x3(out IndexedVector3 outVec, ref IndexedVector3 input, ref IndexedMatrix tr)
{
	IndexedBasisMatrix rot = tr._basis;
	IndexedVector3 r0 = rot._el0;
	IndexedVector3 r1 = rot._el1;
	IndexedVector3 r2 = rot._el2;

	float x = r0.X*input.X + r1.X*input.Y + r2.X*input.Z;
	float y = r0.Y*input.X + r1.Y*input.Y + r2.Y*input.Z;
	float z = r0.Z*input.X + r1.Z*input.Y + r2.Z*input.Z;

	outVec = new IndexedVector3(x, y, z);
}

		public Generic6DofConstraint(RigidBody rbA, RigidBody rbB, ref IndexedMatrix frameInA, ref IndexedMatrix frameInB, bool useLinearReferenceFrameA)
			: base(TypedConstraintType.D6_CONSTRAINT_TYPE, rbA, rbB)
		{
			m_frameInA = frameInA;
			m_frameInB = frameInB;
			m_useLinearReferenceFrameA = useLinearReferenceFrameA;
			m_useOffsetForConstraintFrame = D6_USE_FRAME_OFFSET;
			m_linearLimits = new TranslationalLimitMotor();
			m_angularLimits[0] = new RotationalLimitMotor();
			m_angularLimits[1] = new RotationalLimitMotor();
			m_angularLimits[2] = new RotationalLimitMotor();
			CalculateTransforms();
		}

        public void ConvexSweepTest(ConvexShape castShape, ref IndexedMatrix convexFromWorld, ref IndexedMatrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
        {
            IndexedMatrix convexFromTrans = convexFromWorld;
            IndexedMatrix convexToTrans = convexToWorld;

            IndexedVector3 castShapeAabbMin;
            IndexedVector3 castShapeAabbMax;
            /* Compute AABB that encompasses angular movement */
            IndexedVector3 linVel, angVel;
            TransformUtil.CalculateVelocity(ref convexFromTrans, ref convexToTrans, 1.0f, out linVel, out angVel);

            // FIXME MAN check this - should be a get/set rotation call, basis copy like this may break with scale?
            IndexedMatrix R = IndexedMatrix.Identity;
            R.SetRotation(convexFromTrans.GetRotation());


            castShape.CalculateTemporalAabb(ref R, ref linVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax);

            /// go over all objects, and if the ray intersects their aabb + cast shape aabb,
            // do a ray-shape query using convexCaster (CCD)
            for (int i = 0; i < m_overlappingObjects.Count; i++)
            {
                CollisionObject collisionObject = m_overlappingObjects[i];
                //only perform raycast if filterMask matches
                if (resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle()))
                {
                    //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
                    IndexedVector3 collisionObjectAabbMin;
                    IndexedVector3 collisionObjectAabbMax;
                    IndexedMatrix t = collisionObject.GetWorldTransform();
                    collisionObject.GetCollisionShape().GetAabb(ref t, out collisionObjectAabbMin, out collisionObjectAabbMax);
                    AabbUtil2.AabbExpand(ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax);
                    float hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
                    IndexedVector3 hitNormal;

                    if (AabbUtil2.RayAabb(convexFromWorld._origin, convexToWorld._origin, ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref hitLambda, out hitNormal))
                    {
                        IndexedMatrix wt = collisionObject.GetWorldTransform();
                        CollisionWorld.ObjectQuerySingle(castShape, ref convexFromTrans, ref convexToTrans,
                            collisionObject,
                                collisionObject.GetCollisionShape(),
                                ref wt,
                                resultCallback,
                                allowedCcdPenetration);
                    }
                }
            }

        }

 public virtual void DrawBox(ref IndexedVector3 bbMin, ref IndexedVector3 bbMax, ref IndexedMatrix trans, ref IndexedVector3 color)
 {
     DrawLine(trans * bbMin, trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMin.Z), color);
     DrawLine(trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMin.Z), trans * new IndexedVector3(bbMax.X, bbMax.Y, bbMin.Z), color);
     DrawLine(trans * new IndexedVector3(bbMax.X, bbMax.Y, bbMin.Z), trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMin.Z), color);
     DrawLine(trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMin.Z), trans * bbMin, color);
     DrawLine(trans * bbMin, trans * new IndexedVector3(bbMin.X, bbMin.Y, bbMax.Z), color);
     DrawLine(trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMin.Z), trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMax.Z), color);
     DrawLine(trans * new IndexedVector3(bbMax.X, bbMax.Y, bbMin.Z), trans * bbMax, color);
     DrawLine(trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMin.Z), trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMax.Z), color);
     DrawLine(trans * new IndexedVector3(bbMin.X, bbMin.Y, bbMax.Z), trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMax.Z), color);
     DrawLine(trans * new IndexedVector3(bbMax.X, bbMin.Y, bbMax.Z), trans * bbMax, color);
     DrawLine(trans * bbMax, trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMax.Z), color);
     DrawLine(trans * new IndexedVector3(bbMin.X, bbMax.Y, bbMax.Z), trans * new IndexedVector3(bbMin.X, bbMin.Y, bbMax.Z), color);
 }

        public static void ClipHullAgainstHull(ref IndexedVector3 separatingNormal1, ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, float minDist, float maxDist, IDiscreteCollisionDetectorInterfaceResult resultOut)
        {

            IndexedVector3 separatingNormal = separatingNormal1.Normalized();
            IndexedVector3 c0 = transA * hullA.m_localCenter;
            IndexedVector3 c1 = transB * hullB.m_localCenter;
            IndexedVector3 DeltaC2 = c0 - c1;
            float curMaxDist = maxDist;
            int closestFaceB = -1;
            float dmax = float.MinValue;

            {
                for (int face = 0; face < hullB.m_faces.Count; face++)
                {
                    IndexedVector3 Normal = new IndexedVector3(hullB.m_faces[face].m_plane[0], hullB.m_faces[face].m_plane[1], hullB.m_faces[face].m_plane[2]);
                    IndexedVector3 WorldNormal = transB._basis * Normal;

                    float d = IndexedVector3.Dot(WorldNormal, separatingNormal);
                    if (d > dmax)
                    {
                        dmax = d;
                        closestFaceB = face;
                    }
                }
            }


            // setup initial clip face (minimizing face from hull B)
            ObjectArray<IndexedVector3> worldVertsB1 = new ObjectArray<IndexedVector3>();
            {
                Face polyB = hullB.m_faces[closestFaceB];
                int numVertices = polyB.m_indices.Count;
                for (int e0 = 0; e0 < numVertices; e0++)
                {
                    IndexedVector3 b = hullB.m_vertices[polyB.m_indices[e0]];
                    // check this to see if it is transposed version
                    worldVertsB1.Add(transB * b);
                }
            }
            if (closestFaceB >= 0)
            {
                ClipFaceAgainstHull(ref separatingNormal, hullA, ref transA, worldVertsB1, minDist, maxDist, resultOut);
            }

        }

        public override void GetAabb(ref IndexedMatrix t, out IndexedVector3 aabbMin, out IndexedVector3 aabbMax)
        {
            #if true
	            base.GetAabb(ref t,out aabbMin,out aabbMax);
            #else
            aabbMin = MathUtil.MAX_VECTOR;
            aabbMax = MathUtil.MIN_VECTOR;

	            //just transform the vertices in worldspace, and take their AABB
	            for (int i=0;i<m_numVertices;i++)
	            {
		            IndexedVector3 worldVertex = IndexedVector3.Transformt(m_vertices[i],t);
                    MathUtil.vectorMin(ref worldVertex, ref aabbMin);
                    MathUtil.vectorMin(ref worldVertex,ref aabbMax);
	            }
            #endif

        }

        public static IndexedVector3 GimInertiaAddTransformed(ref IndexedVector3 source_inertia, ref IndexedVector3 added_inertia, ref IndexedMatrix transform)
        {
            //    btMatrix3x3  rotatedTensor = transform.getBasis().scaled(added_inertia) * transform.getBasis().transpose();

            //    float x2 = transform.getOrigin()[0];
            //    x2*= x2;
            //    float y2 = transform.getOrigin()[1];
            //    y2*= y2;
            //    float z2 = transform.getOrigin()[2];
            //    z2*= z2;

            //    float ix = rotatedTensor[0][0]*(y2+z2);
            //    float iy = rotatedTensor[1][1]*(x2+z2);
            //    float iz = rotatedTensor[2][2]*(x2+y2);

            //    return btVector3(source_inertia[0]+ix,source_inertia[1]+iy,source_inertia[2] + iz);
            return IndexedVector3.Zero;
        }

        ///calculateTemporalAabb calculates the enclosing aabb for the moving object over interval [0..timeStep)
	    ///result is conservative
	    public void CalculateTemporalAabb(ref IndexedMatrix curTrans,ref IndexedVector3 linvel,ref IndexedVector3 angvel,float timeStep, out IndexedVector3 temporalAabbMin,out IndexedVector3 temporalAabbMax)
        {
	        //start with static aabb
	        GetAabb(ref curTrans,out temporalAabbMin,out temporalAabbMax);

	        float temporalAabbMaxx = temporalAabbMax.X;
	        float temporalAabbMaxy = temporalAabbMax.Y;
	        float temporalAabbMaxz = temporalAabbMax.Z;
	        float temporalAabbMinx = temporalAabbMin.X;
	        float temporalAabbMiny = temporalAabbMin.Y;
	        float temporalAabbMinz = temporalAabbMin.Z;

	        // add linear motion
	        IndexedVector3 linMotion = linvel*timeStep;
	        ///@todo: simd would have a vector max/min operation, instead of per-element access
	        if (linMotion.X > 0f)
		        temporalAabbMaxx += linMotion.X; 
	        else
		        temporalAabbMinx += linMotion.X;
	        if (linMotion.Y > 0f)
		        temporalAabbMaxy += linMotion.Y; 
	        else
		        temporalAabbMiny += linMotion.Y;
	        if (linMotion.Z > 0f)
		        temporalAabbMaxz += linMotion.Z; 
	        else
		        temporalAabbMinz += linMotion.Z;

	        //add conservative angular motion
	        float angularMotion = angvel.Length() * GetAngularMotionDisc() * timeStep;
            IndexedVector3 angularMotion3d = new IndexedVector3(angularMotion);
	        temporalAabbMin = new IndexedVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz);
	        temporalAabbMax = new IndexedVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz);

	        temporalAabbMin -= angularMotion3d;
	        temporalAabbMax += angularMotion3d;

        }

        public static bool TestSepAxis(ConvexPolyhedron hullA, ConvexPolyhedron hullB, ref IndexedMatrix transA, ref IndexedMatrix transB, ref IndexedVector3 sep_axis, out float depth)
        {
            float Min0, Max0;
            float Min1, Max1;
            hullA.Project(ref transA, ref sep_axis, out Min0, out Max0);
            hullB.Project(ref transB, ref sep_axis, out Min1, out Max1);

            if (Max0 < Min1 || Max1 < Min0)
            {
                depth = 0;
                return false;
            }

            float d0 = Max0 - Min1;
            Debug.Assert(d0 >= 0.0f);
            float d1 = Max1 - Min0;
            Debug.Assert(d1 >= 0.0f);
            depth = d0 < d1 ? d0 : d1;
            return true;
        }