public void DrawArc(ref IndexedVector3 center, ref IndexedVector3 normal, ref IndexedVector3 axis, float radiusA, float radiusB, float minAngle, float maxAngle, ref IndexedVector3 color, bool drawSect, float stepDegrees)
 {
     IndexedVector3 vx = axis;
     IndexedVector3 vy = IndexedVector3.Cross(normal, axis);
     float step = stepDegrees * MathUtil.SIMD_RADS_PER_DEG;
     int nSteps = (int)((maxAngle - minAngle) / step);
     if (nSteps == 0)
     {
         nSteps = 1;
     }
     IndexedVector3 prev = center + radiusA * vx * (float)Math.Cos(minAngle) + radiusB * vy * (float)Math.Sin(minAngle);
     if (drawSect)
     {
         DrawLine(ref center, ref prev, ref color);
     }
     for (int i = 1; i <= nSteps; i++)
     {
         float angle = minAngle + (maxAngle - minAngle) * i / nSteps;
         IndexedVector3 next = center + radiusA * vx * (float)Math.Cos(angle) + radiusB * vy * (float)Math.Sin(angle);
         DrawLine(ref prev, ref next, ref color);
         prev = next;
     }
     if (drawSect)
     {
         DrawLine(ref center, ref prev, ref color);
     }
 }

        public override IndexedVector3 LocalGetSupportingVertexWithoutMargin(ref IndexedVector3 vec)
        {
	        IndexedVector3 supVec = IndexedVector3.Zero;
	        float newDot,maxDot = float.MinValue;
#if DEBUG
			if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConvexHull)
			{
				BulletGlobals.g_streamWriter.WriteLine("localGetSupportingVertexWithoutMargin");
				MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "vec", vec);
				for (int i = 0; i < m_unscaledPoints.Count; ++i)
				{
					MathUtil.PrintVector3(BulletGlobals.g_streamWriter, m_unscaledPoints[i]);
				}
			}
#endif

	        for (int i=0;i<m_unscaledPoints.Count;i++)
	        {
		        IndexedVector3 vtx = m_unscaledPoints[i] * m_localScaling;

		        newDot = IndexedVector3.Dot(vec,vtx);
		        if (newDot > maxDot)
		        {
			        maxDot = newDot;
			        supVec = vtx;
		        }
	        }
	        return supVec;
        }

        public override void ProcessAllTriangles(ITriangleCallback callback, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
        {
	        IndexedVector3 halfExtents = (aabbMax - aabbMin) * .5f;
	        float radius = halfExtents.Length();
	        IndexedVector3 center = (aabbMax + aabbMin) * 0.5f;
        	
	        //this is where the triangles are generated, given AABB and plane equation (normal/constant)

	        IndexedVector3 tangentDir0;
            IndexedVector3 tangentDir1;

	        //tangentDir0/tangentDir1 can be precalculated
	        TransformUtil.PlaneSpace1(ref m_planeNormal, out tangentDir0, out tangentDir1);

            IndexedVector3 supVertex0 = IndexedVector3.Zero;
            IndexedVector3 supVertex1 = IndexedVector3.Zero;

	        IndexedVector3 projectedCenter = center - (IndexedVector3.Dot(m_planeNormal,center) - m_planeConstant)*m_planeNormal;

            IndexedVector3[] triangle = new IndexedVector3[3];
	        triangle[0] = (projectedCenter + tangentDir0*radius + tangentDir1*radius);
            triangle[1] = (projectedCenter + tangentDir0 * radius - tangentDir1 * radius);
            triangle[2] = (projectedCenter - tangentDir0 * radius - tangentDir1 * radius);

	        callback.ProcessTriangle(triangle,0,0);

	        triangle[0] = projectedCenter - tangentDir0*radius - tangentDir1*radius;
	        triangle[1] = projectedCenter - tangentDir0*radius + tangentDir1*radius;
	        triangle[2] = projectedCenter + tangentDir0*radius + tangentDir1*radius;

	        callback.ProcessTriangle(triangle,0,1);

        }

 public void Initialize(ref IndexedVector3 p0,ref IndexedVector3 p1,ref IndexedVector3 p2)
 {
     m_shapeType = BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE;
     m_vertices1[0] = p0;
     m_vertices1[1] = p1;
     m_vertices1[2] = p2;
 }

 public static IndexedVector3 GimGetPointInertia(ref IndexedVector3 point, float mass)
 {
     float x2 = point.X * point.X;
     float y2 = point.Y * point.Y;
     float z2 = point.Z * point.Z;
     return new IndexedVector3(mass * (y2 + z2), mass * (x2 + z2), mass * (x2 + y2));
 }

 public TriangleRaycastCallback() { } // for pool
 public TriangleRaycastCallback(ref IndexedVector3 from, ref IndexedVector3 to, EFlags flags)
 {
     m_from = from;
     m_to = to;
     m_flags = flags;
     m_hitFraction = 1f;
 }

        public void Initialise(ref IndexedVector3 pointA, ref IndexedVector3 pointB, ref IndexedVector3 normal, float distance)
        {
            /* Don't initialize default values twice in C# */
            m_lateralFrictionDir1 = IndexedVector3.Zero;
            m_lateralFrictionDir2 = IndexedVector3.Zero;
            m_lifeTime = 0;
            m_appliedImpulseLateral1 = 0f;
            m_appliedImpulseLateral2 = 0f;
            m_contactMotion1 = 0f;
            m_contactMotion2 = 0f;
            m_contactCFM1 = 0f;
            m_contactCFM2 = 0f;

            m_lateralFrictionInitialized = false;
            m_userPersistentData = null;
            m_appliedImpulse = 0f;
            m_partId0 = 0;
            m_partId1 = 0;
            m_index0 = 0;
            m_index1 = 0;
            m_combinedRestitution = 0f;
            m_combinedFriction = 0f;
            m_positionWorldOnA = IndexedVector3.Zero;
            m_positionWorldOnB = IndexedVector3.Zero;

            m_localPointA = pointA;
            m_localPointB = pointB;
            m_normalWorldOnB = normal;
            m_distance1 = distance;

            m_constraintRow[0].Reset();
            m_constraintRow[1].Reset();
            m_constraintRow[2].Reset();
        }

        public void Reset()
        {
            m_relpos1CrossNormal = IndexedVector3.Zero;
            m_contactNormal = IndexedVector3.Zero;
            m_relpos2CrossNormal = IndexedVector3.Zero;
            m_angularComponentA = IndexedVector3.Zero;
            m_angularComponentB = IndexedVector3.Zero;
            m_appliedPushImpulse = 0f;
            m_appliedImpulse = 0f;
            m_friction = 0f;
            m_jacDiagABInv = 0f;
            m_numConsecutiveRowsPerKernel = 0;
            m_frictionIndex = 0;
            m_solverBodyA = null;
            m_companionIdA = 0;
            m_solverBodyB = null;
            m_companionIdB = 0;
            m_originalContactPoint = null;
            //m_originalContactPointConstraint = null;
            m_rhs = 0f;
            m_cfm = 0;
            m_lowerLimit = 0f;
            m_upperLimit = 0f;
            m_rhsPenetration = 0f;
            m_overrideNumSolverIterations = -1;

        }

        public override IndexedVector3 LocalGetSupportingVertexWithoutMargin(ref IndexedVector3 vec0)
        {
	        IndexedVector3 supVec = IndexedVector3.Zero;

	        IndexedVector3 vec = vec0;
	        float lenSqr = vec.LengthSquared();
	        if (lenSqr < 0.0001f)
	        {
                vec = new IndexedVector3(1, 0, 0);
	        } 
            else
	        {
                float rlen = (1.0f) / (float)Math.Sqrt(lenSqr);
                vec *= rlen;
                //vec.Normalize();
            }

	        LocalSupportVertexCallback supportCallback = new LocalSupportVertexCallback(ref vec);
	        IndexedVector3 aabbMax = new IndexedVector3(float.MaxValue);
            IndexedVector3 aabbMin = -aabbMax;
	        m_stridingMesh.InternalProcessAllTriangles(supportCallback,ref aabbMin,ref aabbMax);
	        supVec = supportCallback.GetSupportVertexLocal();

	        return supVec;

        }

 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 void SetupWorldObjects()
        {
            IndexedVector3 halfExtents = new IndexedVector3(10, 5, 10);
            CollisionShape groundShape = new BoxShape(ref halfExtents);

            IndexedVector3 world1Center = new IndexedVector3(-20, -5, 0);
            IndexedVector3 world2Center = new IndexedVector3(20, -5, 0);

            IndexedMatrix groundTransform1 = IndexedMatrix.CreateTranslation(world1Center);
            IndexedMatrix groundTransform2 = IndexedMatrix.CreateTranslation(world2Center);

            IndexedVector3 halfExtents2 = new IndexedVector3(0.5f);
            CollisionShape smallBoxShape = new BoxShape(ref halfExtents2);

            //IndexedMatrix groundTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0,-10,0));
            float mass = 0f;

            LocalCreateRigidBodyMultiWorld(mass, ref groundTransform1, groundShape ,m_worlds[0]);
            LocalCreateRigidBodyMultiWorld(mass, ref groundTransform2, groundShape, m_worlds[1]);

            mass = 1f;
            for (int i = 0; i < 5; ++i)
            {
                IndexedVector3 offset = new IndexedVector3(0, halfExtents.Y + halfExtents2.Y + (2 * i), 0);

                IndexedMatrix boxTransform1 = IndexedMatrix.CreateTranslation(world1Center+offset);
                IndexedMatrix boxTransform2 = IndexedMatrix.CreateTranslation(world2Center + offset);

                LocalCreateRigidBodyMultiWorld(mass, ref boxTransform1, smallBoxShape, m_worlds[0]);
                LocalCreateRigidBodyMultiWorld(mass, ref boxTransform2, smallBoxShape, m_worlds[1]);
            }

        }

	    ///btActionInterface interface
        public virtual void DebugDraw(IDebugDraw debugDrawer)
        {
	        for (int v=0;v<GetNumWheels();v++)
	        {
		        IndexedVector3 wheelColor = new IndexedVector3(0,1,1);
		        if (GetWheelInfo(v).m_raycastInfo.m_isInContact)
		        {
			        wheelColor = new IndexedVector3(0,0,1);
		        } else
		        {
			        wheelColor= new IndexedVector3(1,0,1);
		        }

		        IndexedVector3 wheelPosWS = GetWheelInfo(v).m_worldTransform._origin;

                IndexedMatrix temp = GetWheelInfo(v).m_worldTransform;
                IndexedVector3 axle = new IndexedVector3(
        GetWheelInfo(v).m_worldTransform._basis._el0[GetRightAxis()],
        GetWheelInfo(v).m_worldTransform._basis._el1[GetRightAxis()],
        GetWheelInfo(v).m_worldTransform._basis._el2[GetRightAxis()]);

		        //debug wheels (cylinders)
		        debugDrawer.DrawLine(wheelPosWS,wheelPosWS+axle,wheelColor);
		        debugDrawer.DrawLine(wheelPosWS,GetWheelInfo(v).m_raycastInfo.m_contactPointWS,wheelColor);

	        }
        }

 public IndexedVector4(IndexedVector3 v,float w)
 {
     X = v.X;
     Y = v.Y;
     Z = v.Z;
     W = w;
 }

        //notice that the vectors should be unit length
		public override void BatchedUnitVectorGetSupportingVertexWithoutMargin(IndexedVector3[] vectors, IndexedVector4[] supportVerticesOut, int numVectors) 
        {
	        for (int i=0;i<numVectors;i++)
	        {
		        supportVerticesOut[i] = IndexedVector4.Zero;
	        }
        }

        public virtual IndexedVector3 LocalGetSupportingVertex(ref IndexedVector3 vec)
        {
	        IndexedVector3 supportVertex;

            IndexedMatrix ident = IndexedMatrix.Identity;

	        SupportVertexCallback supportCallback = new SupportVertexCallback(ref vec,ref ident);

	        IndexedVector3 aabbMax = MathUtil.MAX_VECTOR;
            IndexedVector3 aabbMin = MathUtil.MIN_VECTOR;

            // URGGHHH!
            if (m_inConstructor)
            {
                this.ProcessAllTrianglesCtor(supportCallback, ref aabbMin, ref aabbMax);
            }
            else
            {
                ProcessAllTriangles(supportCallback, ref aabbMin, ref aabbMax);
            }
            
	        supportVertex = supportCallback.GetSupportVertexLocal();
            supportCallback.Cleanup();
	        return supportVertex;

        }

	    public virtual IndexedVector3 GetHalfExtentsWithMargin()
	    {
		    IndexedVector3 halfExtents = GetHalfExtentsWithoutMargin();
		    IndexedVector3 margin = new IndexedVector3(GetMargin());
		    halfExtents += margin;
		    return halfExtents;
	    }

 public static void DebugDrawLine(ref IndexedVector3 from, ref IndexedVector3 to, ref IndexedVector3 color)
 {
     if (BulletGlobals.gDebugDraw != null)
     {
         BulletGlobals.gDebugDraw.DrawLine(ref from, ref to, ref color);
     }
 }

		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 override void CalculateLocalInertia(float mass, out IndexedVector3 inertia)
        {
	        ///this linear upscaling is not realistic, but we don't deal with large mass ratios...
	        IndexedVector3 tmpInertia;
	        m_childConvexShape.CalculateLocalInertia(mass, out tmpInertia);
	        inertia = tmpInertia * m_uniformScalingFactor;
        }

	    public BU_Simplex1to4(ref IndexedVector3 pt0,ref IndexedVector3 pt1,ref IndexedVector3 pt2)
        {
            m_shapeType = BroadphaseNativeTypes.TETRAHEDRAL_SHAPE_PROXYTYPE;
            AddVertex(ref pt0);
            AddVertex(ref pt1);
            AddVertex(ref pt2);
        }