C# LinearMath.IndexedMatrix类代码示例

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