C# BEPUutilities.RigidTransform类代码示例

 public bool HassSolidEntity(Location min, Location max)
 {
     // TODO: Better alg!
     BoundingBox bb = new BoundingBox(min.ToBVector(), max.ToBVector());
     List<BroadPhaseEntry> entries = new List<BroadPhaseEntry>();
     PhysicsWorld.BroadPhase.QueryAccelerator.GetEntries(bb, entries);
     if (entries.Count == 0)
     {
         return false;
     }
     Location center = (max + min) * 0.5;
     Location rel = max - min;
     BoxShape box = new BoxShape((double)rel.X, (double)rel.Y, (double)rel.Z);
     RigidTransform start = new RigidTransform(center.ToBVector(), Quaternion.Identity);
     Vector3 sweep = new Vector3(0, 0, 0.01f);
     RayHit rh;
     foreach (BroadPhaseEntry entry in entries)
     {
         if (entry is EntityCollidable && Collision.ShouldCollide(entry) &&
             entry.CollisionRules.Group != CollisionUtil.Player &&
             entry.ConvexCast(box, ref start, ref sweep, out rh))
         {
             return true;
         }
     }
     return false;
 }

 ///<summary>
 /// Constructs a new compound shape entry using the volume of the shape as a weight.
 ///</summary>
 ///<param name="shape">Shape to use.</param>
 ///<param name="localTransform">Local transform of the shape.</param>
 ///<param name="weight">Weight of the entry.  This defines how much the entry contributes to its owner
 /// for the purposes of center of rotation computation.</param>
 public CompoundShapeEntry(EntityShape shape, RigidTransform localTransform, float weight)
 {
     localTransform.Validate();
     LocalTransform = localTransform;
     Shape = shape;
     Weight = weight;
 }

 public override SolverUpdateable GetBaseJoint()
 {
     RigidTransform rt1 = new RigidTransform(Ent1.Body.Position, Ent1.Body.Orientation);
     RigidTransform rt2 = new RigidTransform(Ent2.Body.Position, Ent2.Body.Orientation);
     RigidTransform.MultiplyByInverse(ref rt1, ref rt2, out Relative);
     return new WeldJoint(Ent1.Body, Ent2.Body);
 }

 public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func<BroadPhaseEntry, bool> filter, out RayHit hit)
 {
     Vector3 swp = sweep;
     double len = swp.Length();
     swp /= len;
     return ConvexCast(castShape, ref startingTransform, ref swp, len, MaterialSolidity.FULLSOLID, out hit);
 }

 void ApplyLiquidForcesTo(Entity e, double dt)
 {
     if (e.Mass <= 0)
     {
         return;
     }
     RigidTransform ert = new RigidTransform(e.Position, e.Orientation);
     BoundingBox entbb;
     e.CollisionInformation.Shape.GetBoundingBox(ref ert, out entbb);
     Location min = new Location(entbb.Min);
     Location max = new Location(entbb.Max);
     min = min.GetBlockLocation();
     max = max.GetUpperBlockBorder();
     for (int x = (int)min.X; x < max.X; x++)
     {
         for (int y = (int)min.Y; y < max.Y; y++)
         {
             for (int z = (int)min.Z; z < max.Z; z++)
             {
                 Location c = new Location(x, y, z);
                 Material mat = (Material)TheRegion.GetBlockInternal_NoLoad(c).BlockMaterial;
                 if (mat.GetSolidity() != MaterialSolidity.LIQUID)
                 {
                     continue;
                 }
                 // TODO: Account for block shape?
                 double vol = e.CollisionInformation.Shape.Volume;
                 double dens = (e.Mass / vol);
                 double WaterDens = 5; // TODO: Read from material. // TODO: Sanity of values.
                 double modifier = (double)(WaterDens / dens);
                 double submod = 0.125f;
                 // TODO: Tracing accuracy!
                 Vector3 impulse = -(TheRegion.PhysicsWorld.ForceUpdater.Gravity + TheRegion.GravityNormal.ToBVector() * 0.4f) * e.Mass * dt * modifier * submod;
                 // TODO: Don't apply smaller logic this if scale is big!
                 for (double x2 = 0.25f; x2 < 1; x2 += 0.5f)
                 {
                     for (double y2 = 0.25f; y2 < 1; y2 += 0.5f)
                     {
                         for (double z2 = 0.25f; z2 < 1; z2 += 0.5f)
                         {
                             Location lc = c + new Location(x2, y2, z2);
                             RayHit rh;
                             if (e.CollisionInformation.RayCast(new Ray(lc.ToBVector(), new Vector3(0, 0, 1)), 0.01f, out rh)) // TODO: Efficiency!
                             {
                                 Vector3 center = lc.ToBVector();
                                 e.ApplyImpulse(ref center, ref impulse);
                                 e.ModifyLinearDamping(mat.GetSpeedMod());
                                 e.ModifyAngularDamping(mat.GetSpeedMod());
                             }
                         }
                     }
                 }
             }
         }
     }
 }

        ///<summary>
        /// Gets the extreme point of the shape in world space in a given direction.
        ///</summary>
        ///<param name="direction">Direction to find the extreme point in.</param>
        /// <param name="shapeTransform">Transform to use for the shape.</param>
        ///<param name="extremePoint">Extreme point on the shape.</param>
        public void GetExtremePointWithoutMargin(Vector3 direction, ref RigidTransform shapeTransform, out Vector3 extremePoint)
        {
            Quaternion conjugate;
            Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate);
            Vector3.Transform(ref direction, ref conjugate, out direction);
            GetLocalExtremePointWithoutMargin(ref direction, out extremePoint);

            Vector3.Transform(ref extremePoint, ref shapeTransform.Orientation, out extremePoint);
            Vector3.Add(ref extremePoint, ref shapeTransform.Position, out extremePoint);
        }

 ///<summary>
 /// Gets the local transform of B in the space of A.
 ///</summary>
 ///<param name="transformA">First transform.</param>
 ///<param name="transformB">Second transform.</param>
 ///<param name="localTransformB">Transform of B in the local space of A.</param>
 public static void GetLocalTransform(ref RigidTransform transformA, ref RigidTransform transformB,
                                      out RigidTransform localTransformB)
 {
     //Put B into A's space.
     Quaternion conjugateOrientationA;
     Quaternion.Conjugate(ref transformA.Orientation, out conjugateOrientationA);
     Quaternion.Concatenate(ref transformB.Orientation, ref conjugateOrientationA, out localTransformB.Orientation);
     Vector3.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position);
     Vector3.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position);
 }

        ///<summary>
        /// Gets the extreme point of the shape in world space in a given direction with margin expansion.
        ///</summary>
        ///<param name="direction">Direction to find the extreme point in.</param>
        /// <param name="shapeTransform">Transform to use for the shape.</param>
        ///<param name="extremePoint">Extreme point on the shape.</param>
        public void GetExtremePoint(Vector3 direction, ref RigidTransform shapeTransform, out Vector3 extremePoint)
        {
            GetExtremePointWithoutMargin(direction, ref shapeTransform, out extremePoint);

            float directionLength = direction.LengthSquared();
            if (directionLength > Toolbox.Epsilon)
            {
                Vector3.Multiply(ref direction, collisionMargin / (float)Math.Sqrt(directionLength), out direction);
                Vector3.Add(ref extremePoint, ref direction, out extremePoint);
            }

        }

 /// <summary>
 /// Gets the bounding box of the shape given a transform.
 /// </summary>
 /// <param name="shapeTransform">Transform to use.</param>
 /// <param name="boundingBox">Bounding box of the transformed shape.</param>
 public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
 {
     #if !WINDOWS
     boundingBox = new BoundingBox();
     #endif
     boundingBox.Min.X = shapeTransform.Position.X - collisionMargin;
     boundingBox.Min.Y = shapeTransform.Position.Y - collisionMargin;
     boundingBox.Min.Z = shapeTransform.Position.Z - collisionMargin;
     boundingBox.Max.X = shapeTransform.Position.X + collisionMargin;
     boundingBox.Max.Y = shapeTransform.Position.Y + collisionMargin;
     boundingBox.Max.Z = shapeTransform.Position.Z + collisionMargin;
 }

        public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
        {
#if !WINDOWS
            boundingBox = new BoundingBox();
#endif
            var upExtreme = new Vector3(0, halfLength, 0);
            var downExtreme = new Vector3(0, -halfLength, 0);

            Vector3.Transform(ref upExtreme, ref shapeTransform.Orientation, out upExtreme);
            Vector3.Transform(ref downExtreme, ref shapeTransform.Orientation, out downExtreme);

            if (upExtreme.X > downExtreme.X)
            {
                boundingBox.Max.X = upExtreme.X;
                boundingBox.Min.X = downExtreme.X;
            }
            else
            {
                boundingBox.Max.X = downExtreme.X;
                boundingBox.Min.X = upExtreme.X;
            }

            if (upExtreme.Y > downExtreme.Y)
            {
                boundingBox.Max.Y = upExtreme.Y;
                boundingBox.Min.Y = downExtreme.Y;
            }
            else
            {
                boundingBox.Max.Y = downExtreme.Y;
                boundingBox.Min.Y = upExtreme.Y;
            }

            if (upExtreme.Z > downExtreme.Z)
            {
                boundingBox.Max.Z = upExtreme.Z;
                boundingBox.Min.Z = downExtreme.Z;
            }
            else
            {
                boundingBox.Max.Z = downExtreme.Z;
                boundingBox.Min.Z = upExtreme.Z;
            }


            boundingBox.Min.X += shapeTransform.Position.X - collisionMargin;
            boundingBox.Min.Y += shapeTransform.Position.Y - collisionMargin;
            boundingBox.Min.Z += shapeTransform.Position.Z - collisionMargin;
            boundingBox.Max.X += shapeTransform.Position.X + collisionMargin;
            boundingBox.Max.Y += shapeTransform.Position.Y + collisionMargin;
            boundingBox.Max.Z += shapeTransform.Position.Z + collisionMargin;
        }

        /// <summary>
        /// Refreshes the contact manifold, removing any out of date contacts
        /// and updating others.
        /// </summary>
        public static void ContactRefresh(RawList<Contact> contacts, RawValueList<ContactSupplementData> supplementData, ref RigidTransform transformA, ref RigidTransform transformB, RawList<int> toRemove)
        {
            //TODO: Could also refresh normals with some trickery.
            //Would also need to refresh depth using new normals, and would require some extra information.

            for (int k = 0; k < contacts.Count; k++)
            {
                contacts.Elements[k].Validate();
                ContactSupplementData data = supplementData.Elements[k];
                System.Numerics.Vector3 newPosA, newPosB;
                RigidTransform.Transform(ref data.LocalOffsetA, ref transformA, out newPosA);
                RigidTransform.Transform(ref data.LocalOffsetB, ref transformB, out newPosB);

                //ab - (ab*n)*n
                //Compute the horizontal offset.
                System.Numerics.Vector3 ab;
                Vector3Ex.Subtract(ref newPosB, ref newPosA, out ab);
                float dot;
                Vector3Ex.Dot(ref ab, ref contacts.Elements[k].Normal, out dot);
                System.Numerics.Vector3 temp;
                Vector3Ex.Multiply(ref contacts.Elements[k].Normal, dot, out temp);
                Vector3Ex.Subtract(ref ab, ref temp, out temp);
                dot = temp.LengthSquared();
                if (dot > CollisionDetectionSettings.ContactInvalidationLengthSquared)
                {
                    toRemove.Add(k);
                }
                else
                {
                    //Depth refresh:
                    //Find deviation ((Ra-Rb)*N) and add to base depth.
                    Vector3Ex.Dot(ref ab, ref contacts.Elements[k].Normal, out dot);
                    contacts.Elements[k].PenetrationDepth = data.BasePenetrationDepth - dot;
                    if (contacts.Elements[k].PenetrationDepth < -CollisionDetectionSettings.maximumContactDistance)
                        toRemove.Add(k);
                    else
                    {
                        //Refresh position and ra/rb.
                        System.Numerics.Vector3 newPos;
                        Vector3Ex.Add(ref newPosB, ref newPosA, out newPos);
                        Vector3Ex.Multiply(ref newPos, .5f, out newPos);
                        contacts.Elements[k].Position = newPos;
                        //This is an interesting idea, but has very little effect one way or the other.
                        //data.BasePenetrationDepth = contacts.Elements[k].PenetrationDepth;
                        //RigidTransform.TransformByInverse(ref newPos, ref transformA, out data.LocalOffsetA);
                        //RigidTransform.TransformByInverse(ref newPos, ref transformB, out data.LocalOffsetB);
                    }
                    contacts.Elements[k].Validate();
                }

            }
        }

        public override void UpdateCollision(float dt)
        {
            WasContaining = Containing;
            WasTouching = Touching;


            var transform = new RigidTransform { Orientation = Quaternion.Identity };
            DetectorVolume.TriangleMesh.Tree.GetOverlaps(convex.boundingBox, overlaps);
            for (int i = 0; i < overlaps.Count; i++)
            {
                DetectorVolume.TriangleMesh.Data.GetTriangle(overlaps.Elements[i], out triangle.vA, out triangle.vB, out triangle.vC);
                Vector3.Add(ref triangle.vA, ref triangle.vB, out transform.Position);
                Vector3.Add(ref triangle.vC, ref transform.Position, out transform.Position);
                Vector3.Multiply(ref transform.Position, 1 / 3f, out transform.Position);
                Vector3.Subtract(ref triangle.vA, ref transform.Position, out triangle.vA);
                Vector3.Subtract(ref triangle.vB, ref transform.Position, out triangle.vB);
                Vector3.Subtract(ref triangle.vC, ref transform.Position, out triangle.vC);

                //If this triangle collides with the convex, we can stop immediately since we know we're touching and not containing.)))
                //[MPR is used here in lieu of GJK because the MPR implementation tends to finish quicker when objects are overlapping than GJK.  The GJK implementation does better on separated objects.]
                if (MPRToolbox.AreShapesOverlapping(convex.Shape, triangle, ref convex.worldTransform, ref transform))
                {
                    Touching = true;
                    //The convex can't be fully contained if it's still touching the surface.
                    Containing = false;

                    overlaps.Clear();
                    goto events;
                }
            }

            overlaps.Clear();
            //If we get here, then there was no shell intersection.
            //If the convex's center point is contained by the mesh, then the convex is fully contained.
            //If this is a child pair, the CheckContainment flag may be set to false.  This is because the parent has
            //already determined that it is not contained (another child performed the check and found that it was not contained)
            //and that it is already touching somehow (either by intersection or by containment).
            //so further containment tests are unnecessary.
            if (CheckContainment && DetectorVolume.IsPointContained(ref convex.worldTransform.Position, overlaps))
            {
                Touching = true;
                Containing = true;
                goto events;
            }

            //If we get here, then there was no surface intersection and the convex's center is not contained- the volume and convex are separate!
            Touching = false;
            Containing = false;

        events:
            NotifyDetectorVolumeOfChanges();
        }

        ///<summary>
        /// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA.
        ///</summary>
        ///<param name="shapeA">First shape.</param>
        ///<param name="shapeB">Second shape.</param>
        ///<param name="direction">Extreme point direction in local space.</param>
        ///<param name="localTransformB">Transform of shapeB in the local space of A.</param>
        ///<param name="extremePoint">The extreme point in the local space of A.</param>
        public static void GetLocalMinkowskiExtremePoint(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 direction, ref RigidTransform localTransformB, out Vector3 extremePoint)
        {
            //Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D)
            shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePoint);
            Vector3 v;
            Vector3 negativeN;
            Vector3.Negate(ref direction, out negativeN);
            shapeB.GetExtremePointWithoutMargin(negativeN, ref localTransformB, out v);
            Vector3.Subtract(ref extremePoint, ref v, out extremePoint);

            ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, ref direction, out v);
            Vector3.Add(ref extremePoint, ref v, out extremePoint);
        }

        /// <summary>
        /// Gets the bounding box of the shape given a transform.
        /// </summary>
        /// <param name="shapeTransform">Transform to use.</param>
        /// <param name="boundingBox">Bounding box of the transformed shape.</param>
        public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
        {
            #if !WINDOWS
            boundingBox = new BoundingBox();
            #endif
            Matrix3x3 o;
            Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
            //Sample the local directions from the orientation matrix, implicitly transposed.

            Vector3 right;
            var direction = new Vector3(o.M11, o.M21, o.M31);
            GetLocalExtremePointWithoutMargin(ref direction, out right);

            Vector3 left;
            direction = new Vector3(-o.M11, -o.M21, -o.M31);
            GetLocalExtremePointWithoutMargin(ref direction, out left);

            Vector3 up;
            direction = new Vector3(o.M12, o.M22, o.M32);
            GetLocalExtremePointWithoutMargin(ref direction, out up);

            Vector3 down;
            direction = new Vector3(-o.M12, -o.M22, -o.M32);
            GetLocalExtremePointWithoutMargin(ref direction, out down);

            Vector3 backward;
            direction = new Vector3(o.M13, o.M23, o.M33);
            GetLocalExtremePointWithoutMargin(ref direction, out backward);

            Vector3 forward;
            direction = new Vector3(-o.M13, -o.M23, -o.M33);
            GetLocalExtremePointWithoutMargin(ref direction, out forward);

            //Rather than transforming each axis independently (and doing three times as many operations as required), just get the 6 required values directly.
            Vector3 positive, negative;
            TransformLocalExtremePoints(ref right, ref up, ref backward, ref o, out positive);
            TransformLocalExtremePoints(ref left, ref down, ref forward, ref o, out negative);

            //The positive and negative vectors represent the X, Y and Z coordinates of the extreme points in world space along the world space axes.
            boundingBox.Max.X = shapeTransform.Position.X + positive.X + collisionMargin;
            boundingBox.Max.Y = shapeTransform.Position.Y + positive.Y + collisionMargin;
            boundingBox.Max.Z = shapeTransform.Position.Z + positive.Z + collisionMargin;

            boundingBox.Min.X = shapeTransform.Position.X + negative.X - collisionMargin;
            boundingBox.Min.Y = shapeTransform.Position.Y + negative.Y - collisionMargin;
            boundingBox.Min.Z = shapeTransform.Position.Z + negative.Z - collisionMargin;
        }

 public MobileChunkShape(Vector3i csize, BlockInternal[] blocks, out Vector3 center)
 {
     Matrix3x3 boxMat = new BoxShape(csize.X, csize.Y, csize.Z).VolumeDistribution;
     ChunkSize = csize;
     Blocks = blocks;
     double weightInv = 1f / blocks.Length;
     center = new Vector3(csize.X / 2f, csize.Y / 2f, csize.Z / 2f);
     // TODO: More accurately get center of weight based on which blocks are solid or not!?
     Matrix3x3 volumeDistribution = new Matrix3x3();
     RigidTransform transform = new RigidTransform(center);
     Matrix3x3 contribution;
     CompoundShape.TransformContribution(ref transform, ref center, ref boxMat, blocks.Length, out contribution);
     Matrix3x3.Add(ref volumeDistribution, ref contribution, out volumeDistribution);
     Matrix3x3.Multiply(ref volumeDistribution, weightInv, out volumeDistribution);
     UpdateEntityShapeVolume(new EntityShapeVolumeDescription() { Volume = csize.X * csize.Y * csize.Z, VolumeDistribution = volumeDistribution });
     Center = center;
 }