Esempio n. 1
0
        /// <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.
            //Notice only three directions are used.  Due to box symmetry, 'left' is just -right.
            var right = new Vector3(Math.Sign(o.M11) * halfWidth, Math.Sign(o.M21) * halfHeight, Math.Sign(o.M31) * halfLength);

            var up = new Vector3(Math.Sign(o.M12) * halfWidth, Math.Sign(o.M22) * halfHeight, Math.Sign(o.M32) * halfLength);

            var backward = new Vector3(Math.Sign(o.M13) * halfWidth, Math.Sign(o.M23) * halfHeight, Math.Sign(o.M33) * halfLength);

            Matrix3x3.Transform(ref right, ref o, out right);
            Matrix3x3.Transform(ref up, ref o, out up);
            Matrix3x3.Transform(ref backward, ref o, out backward);
            //These right/up/backward represent the extreme points in world space along the world space axes.

            boundingBox.Max.X = shapeTransform.Position.X + right.X;
            boundingBox.Max.Y = shapeTransform.Position.Y + up.Y;
            boundingBox.Max.Z = shapeTransform.Position.Z + backward.Z;

            boundingBox.Min.X = shapeTransform.Position.X - right.X;
            boundingBox.Min.Y = shapeTransform.Position.Y - up.Y;
            boundingBox.Min.Z = shapeTransform.Position.Z - backward.Z;
        }
Esempio n. 2
0
        protected override void ConfigureCollidable(TriangleEntry entry, Fix64 dt)
        {
            var shape = entry.Collidable.Shape;

            mesh.Shape.TriangleMesh.Data.GetTriangle(entry.Index, out shape.vA, out shape.vB, out shape.vC);
            Matrix3x3 o;

            Matrix3x3.CreateFromQuaternion(ref mesh.worldTransform.Orientation, out o);
            Matrix3x3.Transform(ref shape.vA, ref o, out shape.vA);
            Matrix3x3.Transform(ref shape.vB, ref o, out shape.vB);
            Matrix3x3.Transform(ref shape.vC, ref o, out shape.vC);
            Vector3 center;

            Vector3.Add(ref shape.vA, ref shape.vB, out center);
            Vector3.Add(ref center, ref shape.vC, out center);
            Vector3.Multiply(ref center, F64.OneThird, out center);
            Vector3.Subtract(ref shape.vA, ref center, out shape.vA);
            Vector3.Subtract(ref shape.vB, ref center, out shape.vB);
            Vector3.Subtract(ref shape.vC, ref center, out shape.vC);

            Vector3.Add(ref center, ref mesh.worldTransform.Position, out center);
            //The bounding box doesn't update by itself.
            entry.Collidable.worldTransform.Position    = center;
            entry.Collidable.worldTransform.Orientation = Quaternion.Identity;
            entry.Collidable.UpdateBoundingBoxInternal(dt);
        }
Esempio n. 3
0
        /// <summary>
        /// Modifies a contribution using a transform, position, and weight.
        /// </summary>
        /// <param name="transform">Transform to use to modify the contribution.</param>
        /// <param name="center">Center to use to modify the contribution.</param>
        /// <param name="baseContribution">Original unmodified contribution.</param>
        /// <param name="weight">Weight of the contribution.</param>
        /// <param name="contribution">Transformed contribution.</param>
        public static void TransformContribution(ref RigidTransform transform, ref Vector3 center,
                                                 ref Matrix3x3 baseContribution, float weight, out Matrix3x3 contribution)
        {
            Matrix3x3 rotation;

            Matrix3x3.CreateFromQuaternion(ref transform.Orientation, out rotation);
            Matrix3x3 temp;

            //Do angular transformed contribution first...
            Matrix3x3.MultiplyTransposed(ref rotation, ref baseContribution, out temp);
            Matrix3x3.Multiply(ref temp, ref rotation, out temp);

            contribution = temp;

            //Now add in the offset from the origin.
            Vector3 offset;

            Vector3.Subtract(ref transform.Position, ref center, out offset);
            Matrix3x3 innerProduct;

            Matrix3x3.CreateScale(offset.LengthSquared(), out innerProduct);
            Matrix3x3 outerProduct;

            Matrix3x3.CreateOuterProduct(ref offset, ref offset, out outerProduct);

            Matrix3x3.Subtract(ref innerProduct, ref outerProduct, out temp);

            Matrix3x3.Add(ref contribution, ref temp, out contribution);
            Matrix3x3.Multiply(ref contribution, weight, out contribution);
        }
        /// <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.
            //Notice only three directions are used.  Due to box symmetry, 'left' is just -right.
            var right = new Vector3(Math.Sign(o.M11) * halfWidth, Math.Sign(o.M21) * halfHeight, Math.Sign(o.M31) * halfLength);

            var up = new Vector3(Math.Sign(o.M12) * halfWidth, Math.Sign(o.M22) * halfHeight, Math.Sign(o.M32) * halfLength);

            var backward = new Vector3(Math.Sign(o.M13) * halfWidth, Math.Sign(o.M23) * halfHeight, Math.Sign(o.M33) * halfLength);


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

            //The positive and negative vectors represent the X, Y and Z coordinates of the extreme points in world space along the world space axes.
            Vector3.Add(ref shapeTransform.Position, ref offset, out boundingBox.Max);
            Vector3.Subtract(ref shapeTransform.Position, ref offset, out boundingBox.Min);
        }
        void IPositionUpdateable.PreUpdatePosition(float dt)
        {
            Vector3 increment;

            Vector3.Multiply(ref angularVelocity, dt * .5f, out increment);
            var multiplier = new Quaternion(increment.X, increment.Y, increment.Z, 0);

            Quaternion.Multiply(ref multiplier, ref orientation, out multiplier);
            Quaternion.Add(ref orientation, ref multiplier, out orientation);
            orientation.Normalize();

            Matrix3x3.CreateFromQuaternion(ref orientation, out orientationMatrix);

            //Only do the linear motion if this object doesn't obey CCD.
            if (PositionUpdateMode == PositionUpdateMode.Discrete)
            {
                Vector3.Multiply(ref linearVelocity, dt, out increment);
                Vector3.Add(ref position, ref increment, out position);

                collisionInformation.UpdateWorldTransform(ref position, ref orientation);
                //The position update is complete if this is a discretely updated object.
                if (PositionUpdated != null)
                {
                    PositionUpdated(this);
                }
            }

            MathChecker.Validate(linearVelocity);
            MathChecker.Validate(angularVelocity);
            MathChecker.Validate(position);
            MathChecker.Validate(orientation);
#if CONSERVE
            MathChecker.Validate(angularMomentum);
#endif
        }
Esempio n. 6
0
        /// <summary>
        /// Creates an affine transform from a rigid transform.
        /// </summary>
        /// <param name="rigid">Rigid transform to base the affine transform on.</param>
        /// <returns>Affine transform created from the rigid transform.</returns>
        public static AffineTransform CreateFromRigidTransform(RigidTransform rigid)
        {
            AffineTransform toReturn;

            toReturn.Translation = rigid.Position;
            Matrix3x3.CreateFromQuaternion(ref rigid.Orientation, out toReturn.LinearTransform);
            return(toReturn);
        }
Esempio n. 7
0
        ///<summary>
        /// Updates the cached simplex with the latest run's results.
        ///</summary>
        ///<param name="simplex">Simplex to update.</param>
        public void UpdateCachedSimplex(ref CachedSimplex simplex)
        {
            simplex.LocalSimplexA = SimplexA;
            switch (State)
            {
            case SimplexState.Point:
                Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
                Quaternion conjugate;
                Quaternion.Conjugate(ref LocalTransformB.Orientation, out conjugate);
                Quaternion.Transform(ref simplex.LocalSimplexB.A, ref conjugate, out simplex.LocalSimplexB.A);
                break;

            case SimplexState.Segment:
                Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
                Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);

                Matrix3x3 transform;
                Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
                Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform,
                                             out simplex.LocalSimplexB.A);
                Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform,
                                             out simplex.LocalSimplexB.B);
                break;

            case SimplexState.Triangle:
                Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
                Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
                Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C);

                Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
                Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform,
                                             out simplex.LocalSimplexB.A);
                Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform,
                                             out simplex.LocalSimplexB.B);
                Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform,
                                             out simplex.LocalSimplexB.C);
                break;

            case SimplexState.Tetrahedron:
                Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A);
                Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B);
                Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C);
                Vector3.Subtract(ref SimplexB.D, ref LocalTransformB.Position, out simplex.LocalSimplexB.D);

                Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform);
                Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform,
                                             out simplex.LocalSimplexB.A);
                Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform,
                                             out simplex.LocalSimplexB.B);
                Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform,
                                             out simplex.LocalSimplexB.C);
                Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.D, ref transform,
                                             out simplex.LocalSimplexB.D);
                break;
            }

            simplex.State = State;
        }
Esempio n. 8
0
 public static void RotateInverseInertia(ref Symmetric3x3 localInverseInertiaTensor, ref Quaternion orientation, out Symmetric3x3 rotatedInverseInertiaTensor)
 {
     Matrix3x3.CreateFromQuaternion(orientation, out var orientationMatrix);
     //I^-1 = RT * Ilocal^-1 * R
     //NOTE: If you were willing to confuse users a little bit, the local inertia could be required to be diagonal.
     //This would be totally fine for all the primitive types which happen to have diagonal inertias, but for more complex shapes (convex hulls, meshes),
     //there would need to be a reorientation step. That could be confusing, and it's probably not worth it.
     Symmetric3x3.RotationSandwich(orientationMatrix, localInverseInertiaTensor, out rotatedInverseInertiaTensor);
 }
Esempio n. 9
0
        /// <summary>
        /// Updates the world inertia tensor based upon the local inertia tensor and current orientation.
        /// </summary>
        internal void UpdateInertiaTensor()
        {
            //This is separate from the position update because the orientation can change outside of our iteration loop, so this has to run first.
            //Iworld^-1 = RT * Ilocal^1 * R
            Matrix3x3 orientationMatrix;

            Matrix3x3.CreateFromQuaternion(ref Orientation, out orientationMatrix);
            Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensorInverse, out inertiaTensorInverse);
            Matrix3x3.Multiply(ref inertiaTensorInverse, ref orientationMatrix, out inertiaTensorInverse);
        }
Esempio n. 10
0
        ///<summary>
        /// Constructs a new affine transform.
        ///</summary>
        ///<param name="scaling">Scaling to apply in the linear transform.</param>
        ///<param name="orientation">Orientation to apply in the linear transform.</param>
        ///<param name="translation">Translation to apply.</param>
        public AffineTransform(ref Vector3 scaling, ref Quaternion orientation, ref Vector3 translation)
        {
            //Create an SRT transform.
            Matrix3x3.CreateScale(ref scaling, out LinearTransform);
            Matrix3x3 rotation;

            Matrix3x3.CreateFromQuaternion(ref orientation, out rotation);
            Matrix3x3.Multiply(ref LinearTransform, ref rotation, out LinearTransform);
            Translation = translation;
        }
Esempio n. 11
0
        ///<summary>
        /// Computes the bounding box of the transformed mesh shape.
        ///</summary>
        ///<param name="shapeTransform">Transform to apply to the shape during the bounding box calculation.</param>
        ///<param name="boundingBox">Bounding box containing the transformed mesh shape.</param>
        public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
        {
            //TODO: Could use an approximate bounding volume.  Would be cheaper at runtime and use less memory, though the box would be bigger.
            Matrix3x3 o;

            Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
            GetBoundingBox(ref o, out boundingBox);

            Vector3.Add(ref boundingBox.Max, ref shapeTransform.Position, out boundingBox.Max);
            Vector3.Add(ref boundingBox.Min, ref shapeTransform.Position, out boundingBox.Min);
        }
        public unsafe void ExtractLines(ref LinearAxisServoPrestepData prestepBundle, int setIndex, int *bodyIndices,
                                        Bodies bodies, ref Vector3 tint, ref QuickList <LineInstance> lines)
        {
            //Could do bundles of constraints at a time, but eh.
            var poseA = bodies.Sets[setIndex].Poses[bodyIndices[0]];
            var poseB = bodies.Sets[setIndex].Poses[bodyIndices[1]];

            Vector3Wide.ReadFirst(prestepBundle.LocalOffsetA, out var localOffsetA);
            Vector3Wide.ReadFirst(prestepBundle.LocalOffsetB, out var localOffsetB);
            Vector3Wide.ReadFirst(prestepBundle.LocalPlaneNormal, out var localPlaneNormal);
            var targetOffset = GatherScatter.GetFirst(ref prestepBundle.TargetOffset);

            Matrix3x3.CreateFromQuaternion(poseA.Orientation, out var orientationA);
            Matrix3x3.Transform(localOffsetA, orientationA, out var worldOffsetA);
            Matrix3x3.Transform(localPlaneNormal, orientationA, out var worldPlaneNormal);
            Quaternion.Transform(localOffsetB, poseB.Orientation, out var worldOffsetB);

            var anchorA             = poseA.Position + worldOffsetA;
            var anchorB             = poseB.Position + worldOffsetB;
            var planeOffset         = Vector3.Dot(anchorB - anchorA, worldPlaneNormal);
            var closestPointOnPlane = anchorB - planeOffset * worldPlaneNormal;

            var packedColor      = Helpers.PackColor(new Vector3(0.2f, 0.2f, 1f) * tint);
            var packedBasisColor = Helpers.PackColor(new Vector3(0.2f, 0.6f, 1f) * tint);
            var backgroundColor  = new Vector3(0f, 0f, 1f) * tint;

            lines.AllocateUnsafely() = new LineInstance(poseA.Position, anchorA, packedColor, 0);
            ContactLines.BuildOrthnormalBasis(localPlaneNormal, out var localTX, out var localTY);
            Matrix3x3.Transform(localTX, orientationA, out var tX);
            Matrix3x3.Transform(localTY, orientationA, out var tY);
            lines.AllocateUnsafely() = new LineInstance(anchorA - tX, anchorA + tX, packedBasisColor, 0);
            lines.AllocateUnsafely() = new LineInstance(anchorA - tY, anchorA + tY, packedBasisColor, 0);
            lines.AllocateUnsafely() = new LineInstance(anchorA, closestPointOnPlane, packedColor, 0);
            lines.AllocateUnsafely() = new LineInstance(anchorB, poseB.Position, packedColor, 0);
            if (targetOffset < 0)
            {
                targetOffset     = -targetOffset;
                planeOffset      = -planeOffset;
                worldPlaneNormal = -worldPlaneNormal;
            }
            var targetPoint      = closestPointOnPlane + worldPlaneNormal * targetOffset;
            var packedErrorColor = Helpers.PackColor(new Vector3(1, 0, 0) * tint);

            if (planeOffset > targetOffset)
            {
                lines.AllocateUnsafely() = new LineInstance(closestPointOnPlane, targetPoint, packedColor, 0);
                lines.AllocateUnsafely() = new LineInstance(targetPoint, anchorB, packedErrorColor, 0);
            }
            else
            {
                lines.AllocateUnsafely() = new LineInstance(closestPointOnPlane, anchorB, packedColor, 0);
                lines.AllocateUnsafely() = new LineInstance(anchorB, targetPoint, packedErrorColor, 0);
            }
        }
        ///<summary>
        /// Computes the bounding box of the transformed mesh shape.
        ///</summary>
        ///<param name="shapeTransform">Transform to apply to the shape during the bounding box calculation.</param>
        ///<param name="boundingBox">Bounding box containing the transformed mesh shape.</param>
        public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
        {
            ////TODO: Could use an approximate bounding volume.  Would be cheaper at runtime and use less memory, though the box would be bigger.
            //Matrix3X3 o;
            //Matrix3X3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
            ////Sample the local directions from the orientation matrix, implicitly transposed.
            //Vector3 right = new Vector3(o.M11 * 100000, o.M21 * 100000, o.M31 * 100000);
            //Vector3 up = new Vector3(o.M12 * 100000, o.M22 * 100000, o.M32 * 100000);
            //Vector3 backward = new Vector3(o.M13 * 100000, o.M23 * 100000, o.M33 * 100000);
            //Vector3 left, down, forward;
            //Vector3.Negate(ref right, out left);
            //Vector3.Negate(ref up, out down);
            //Vector3.Negate(ref backward, out forward);
            //for (int i = 0; i < extents.count; i++)
            //{
            //    extents.Elements[i].Clamp(ref right);
            //    extents.Elements[i].Clamp(ref left);
            //    extents.Elements[i].Clamp(ref up);
            //    extents.Elements[i].Clamp(ref down);
            //    extents.Elements[i].Clamp(ref backward);
            //    extents.Elements[i].Clamp(ref forward);
            //}

            //Matrix3X3.Transform(ref right, ref o, out right);
            //Matrix3X3.Transform(ref left, ref o, out left);
            //Matrix3X3.Transform(ref down, ref o, out down);
            //Matrix3X3.Transform(ref up, ref o, out up);
            //Matrix3X3.Transform(ref forward, ref o, out forward);
            //Matrix3X3.Transform(ref backward, ref o, out backward);


            //boundingBox.Max.X = shapeTransform.Position.X + right.X;
            //boundingBox.Max.Y = shapeTransform.Position.Y + up.Y;
            //boundingBox.Max.Z = shapeTransform.Position.Z + backward.Z;

            //boundingBox.Min.X = shapeTransform.Position.X + left.X;
            //boundingBox.Min.Y = shapeTransform.Position.Y + down.Y;
            //boundingBox.Min.Z = shapeTransform.Position.Z + forward.Z;


            Matrix3x3 o;

            Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
            GetBoundingBox(ref o, out boundingBox);


            boundingBox.Max.X += shapeTransform.Position.X;
            boundingBox.Max.Y += shapeTransform.Position.Y;
            boundingBox.Max.Z += shapeTransform.Position.Z;

            boundingBox.Min.X += shapeTransform.Position.X;
            boundingBox.Min.Y += shapeTransform.Position.Y;
            boundingBox.Min.Z += shapeTransform.Position.Z;
        }
Esempio n. 14
0
        ///<summary>
        /// Multiplies a rigid transform by an affine transform.
        ///</summary>
        ///<param name="a">Rigid transform.</param>
        ///<param name="b">Affine transform.</param>
        ///<param name="transform">Combined transform.</param>
        public static void Multiply(ref RigidTransform a, ref AffineTransform b, out AffineTransform transform)
        {
            Matrix3x3 linearTransform;//Have to use temporary variable just in case b reference is transform.

            Matrix3x3.CreateFromQuaternion(ref a.Orientation, out linearTransform);
            Matrix3x3.Multiply(ref linearTransform, ref b.LinearTransform, out linearTransform);
            System.Numerics.Vector3 translation;
            Matrix3x3.Transform(ref a.Position, ref b.LinearTransform, out translation);
            Vector3Ex.Add(ref translation, ref b.Translation, out transform.Translation);
            transform.LinearTransform = linearTransform;
        }
Esempio n. 15
0
        /// <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);


            Matrix3x3.Transform(ref right, ref o, out right);
            Matrix3x3.Transform(ref left, ref o, out left);
            Matrix3x3.Transform(ref up, ref o, out up);
            Matrix3x3.Transform(ref down, ref o, out down);
            Matrix3x3.Transform(ref backward, ref o, out backward);
            Matrix3x3.Transform(ref forward, ref o, out forward);

            //These right/up/backward represent the extreme points in world space along the world space axes.

            boundingBox.Max.X = shapeTransform.Position.X + collisionMargin + right.X;
            boundingBox.Max.Y = shapeTransform.Position.Y + collisionMargin + up.Y;
            boundingBox.Max.Z = shapeTransform.Position.Z + collisionMargin + backward.Z;

            boundingBox.Min.X = shapeTransform.Position.X - collisionMargin + left.X;
            boundingBox.Min.Y = shapeTransform.Position.Y - collisionMargin + down.Y;
            boundingBox.Min.Z = shapeTransform.Position.Z - collisionMargin + forward.Z;
        }
        /// <summary>
        /// Constructs a new demo.
        /// </summary>
        /// <param name="game">Game owning this demo.</param>
        public InverseKinematicsTestDemo2(DemosGame game)
            : base(game)
        {
            game.Camera.Position = new Vector3(0, 3, 5);
            Box ground = new Box(new Vector3(0, -3, 0), 30, 1, 30);

            Space.Add(ground);
            Space.ForceUpdater.Gravity = new Vector3(0, -9.81m, 0);

            var solver = new IKSolver();

            solver.ActiveSet.UseAutomass = true;
            //solver.AutoscaleControlImpulses = true;
            //solver.AutoscaleControlMaximumForce = Fix64.MaxValue;
            solver.ControlIterationCount     = 20;
            solver.FixerIterationCount       = 0;
            solver.VelocitySubiterationCount = 3;


            List <Bone>   bones;
            List <Entity> boneEntities;
            int           boneCount = 10;

            BuildStick(new Vector3(0, 0.5m, 0), boneCount, out bones, out boneEntities);

            DragControl dragger = new DragControl {
                TargetBone = bones[boneCount - 1], MaximumForce = Fix64.MaxValue
            };

            dragger.LinearMotor.Rigidity       = 16;
            dragger.LinearMotor.LocalOffset    = new Vector3(0, 0.5m, 0);
            dragger.LinearMotor.TargetPosition = new Vector3(10, 0, 0);


            bones[0].Pinned = true;

            var controls = new List <Control>();

            controls.Add(dragger);

            solver.Solve(controls);

            var tipLocation = bones[boneCount - 1].Position + Matrix3x3.CreateFromQuaternion(bones[boneCount - 1].Orientation).Up * 0.5m;

            for (int i = 0; i < bones.Count; ++i)
            {
                boneEntities[i].Position    = bones[i].Position;
                boneEntities[i].Orientation = bones[i].Orientation;
                Space.Add(boneEntities[i]);
            }
        }
Esempio n. 17
0
        private void IntegrateCallback(object obj)
        {
            RigidBody body = obj as RigidBody;

            Vector3.Multiply(ref body.linearVelocity, timestep, out Vector3 temp);
            Vector3.Add(ref temp, ref body.position, out body.position);

            if (!(body.isParticle))
            {
                //exponential map
                Vector3 axis;
                float   angle = body.angularVelocity.magnitude;

                if (angle < 0.001f)
                {
                    // use Taylor's expansions of sync function
                    // axis = body.angularVelocity * (0.5f * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle);
                    Vector3.Multiply(ref body.angularVelocity, (0.5f * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle), out axis);
                }
                else
                {
                    // sync(fAngle) = sin(c*fAngle)/t
                    Vector3.Multiply(ref body.angularVelocity, ((float)Math.Sin(0.5f * angle * timestep) / angle), out axis);
                }

                Quaternion dorn = new Quaternion(axis.x, axis.y, axis.z, (float)Math.Cos(angle * timestep * 0.5f));
                Quaternion.CreateFromMatrix(ref body.orientation, out Quaternion ornA);

                Quaternion.Multiply(ref dorn, ref ornA, out dorn);

                dorn.Normalize(); Matrix3x3.CreateFromQuaternion(ref dorn, out body.orientation);
            }

            if ((body.Damping & RigidBody.DampingType.Linear) != 0)
            {
                Vector3.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
            }

            if ((body.Damping & RigidBody.DampingType.Angular) != 0)
            {
                Vector3.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);
            }

            body.Update();


            if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
            {
                body.SweptExpandBoundingBox(timestep);
            }
        }
Esempio n. 18
0
        /// <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;
        }
Esempio n. 19
0
        /// <summary>
        /// Gets the intersection between the triangle and the ray.
        /// </summary>
        /// <param name="ray">Ray to test against the triangle.</param>
        /// <param name="transform">Transform to apply to the triangle shape for the test.</param>
        /// <param name="maximumLength">Maximum distance to travel in units of the direction vector's length.</param>
        /// <param name="hit">Hit data of the ray cast, if any.</param>
        /// <returns>Whether or not the ray hit the target.</returns>
        public override bool RayTest(ref Ray ray, ref RigidTransform transform, Fix64 maximumLength, out RayHit hit)
        {
            Matrix3x3 orientation;
            Matrix3x3.CreateFromQuaternion(ref transform.Orientation, out orientation);
            Ray localRay;
            Quaternion conjugate;
            Quaternion.Conjugate(ref transform.Orientation, out conjugate);
            Quaternion.Transform(ref ray.Direction, ref conjugate, out localRay.Direction);
            Vector3.Subtract(ref ray.Position, ref transform.Position, out localRay.Position);
            Quaternion.Transform(ref localRay.Position, ref conjugate, out localRay.Position);

            bool toReturn = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref vA, ref vB, ref vC, out hit);
            //Move the hit back into world space.
            Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location);
            Vector3.Add(ref ray.Position, ref hit.Location, out hit.Location);
            Quaternion.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
            return toReturn;
        }
Esempio n. 20
0
        /// <summary>
        /// Recalculates the bounding box of the fluid based on its depth, surface normal, and surface triangles.
        /// </summary>
        public void RecalculateBoundingBox()
        {
            var points = CommonResources.GetVectorList();

            foreach (var tri in SurfaceTriangles)
            {
                points.Add(tri[0]);
                points.Add(tri[1]);
                points.Add(tri[2]);
                points.Add(tri[0] - upVector * MaxDepth);
                points.Add(tri[1] - upVector * MaxDepth);
                points.Add(tri[2] - upVector * MaxDepth);
            }
            boundingBox = BoundingBox.CreateFromPoints(points);
            CommonResources.GiveBack(points);

            //Compute the transforms used to pull objects into fluid local space.
            QuaternionEx.GetQuaternionBetweenNormalizedVectors(ref Toolbox.UpVector, ref upVector, out surfaceTransform.Orientation);
            Matrix3x3.CreateFromQuaternion(ref surfaceTransform.Orientation, out toSurfaceRotationMatrix);
            surfaceTransform.Position = surfaceTriangles[0][0];
        }
Esempio n. 21
0
        void IPositionUpdateable.PreUpdatePosition(float dt)
        {
            Vector3 increment;

            if (MotionSettings.UseRk4AngularIntegration && isDynamic)
            {
                Toolbox.UpdateOrientationRK4(ref orientation, ref localInertiaTensorInverse, ref angularMomentum, dt, out orientation);
            }
            else
            {
                Vector3.Multiply(ref angularVelocity, dt * .5f, out increment);
                var multiplier = new Quaternion(increment.X, increment.Y, increment.Z, 0);
                Quaternion.Multiply(ref multiplier, ref orientation, out multiplier);
                Quaternion.Add(ref orientation, ref multiplier, out orientation);
                orientation.Normalize();
            }
            Matrix3x3.CreateFromQuaternion(ref orientation, out orientationMatrix);

            //Only do the linear motion if this object doesn't obey CCD.
            if (PositionUpdateMode == PositionUpdateMode.Discrete)
            {
                Vector3.Multiply(ref linearVelocity, dt, out increment);
                Vector3.Add(ref position, ref increment, out position);

                collisionInformation.UpdateWorldTransform(ref position, ref orientation);
                //The position update is complete if this is a discretely updated object.
                if (PositionUpdated != null)
                {
                    PositionUpdated(this);
                }
            }
            collisionInformation.UpdateWorldTransform(ref position, ref orientation);

            MathChecker.Validate(linearMomentum);
            MathChecker.Validate(linearVelocity);
            MathChecker.Validate(angularMomentum);
            MathChecker.Validate(angularVelocity);
            MathChecker.Validate(position);
            MathChecker.Validate(orientation);
        }
        ///<summary>
        /// Tests a ray against the surface of the mesh.  This does not take into account solidity.
        ///</summary>
        ///<param name="ray">Ray to test.</param>
        ///<param name="maximumLength">Maximum length of the ray to test; in units of the ray's direction's length.</param>
        ///<param name="sidedness">Sidedness to use during the ray cast.  This does not have to be the same as the mesh's sidedness.</param>
        ///<param name="rayHit">The hit location of the ray on the mesh, if any.</param>
        ///<returns>Whether or not the ray hit the mesh.</returns>
        public bool RayCast(Ray ray, Fix64 maximumLength, TriangleSidedness sidedness, out RayHit rayHit)
        {
            //Put the ray into local space.
            Ray       localRay;
            Matrix3x3 orientation;

            Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out orientation);
            Matrix3x3.TransformTranspose(ref ray.Direction, ref orientation, out localRay.Direction);
            Vector3.Subtract(ref ray.Position, ref worldTransform.Position, out localRay.Position);
            Matrix3x3.TransformTranspose(ref localRay.Position, ref orientation, out localRay.Position);

            if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit))
            {
                //Transform the hit into world space.
                Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location);
                Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location);
                Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal);
                return(true);
            }
            rayHit = new RayHit();
            return(false);
        }
Esempio n. 23
0
            public void Test(Random random, int innerIterations)
            {
                Quaternion q;

                q.X = (float)random.NextDouble() * 2 - 1;
                q.Y = (float)random.NextDouble() * 2 - 1;
                q.Z = (float)random.NextDouble() * 2 - 1;
                q.W = (float)random.NextDouble() * 2 - 1;

                Quaternion.NormalizeRef(ref q);

                for (int i = 0; i < innerIterations; ++i)
                {
                    Matrix3x3.CreateFromQuaternion(q, out var r);
                    Quaternion.CreateFromRotationMatrix(r, out var qTest);

#if DEBUG
                    const float epsilon = 1e-6f;
                    var         lengthX = r.X.Length();
                    var         lengthY = r.Y.Length();
                    var         lengthZ = r.Z.Length();
                    Debug.Assert(
                        Math.Abs(1 - lengthX) < epsilon &&
                        Math.Abs(1 - lengthY) < epsilon &&
                        Math.Abs(1 - lengthZ) < epsilon);


                    if (qTest.X * q.X < 0)
                    {
                        Quaternion.Negate(qTest, out qTest);
                    }
                    Debug.Assert(
                        Math.Abs(qTest.X - q.X) < epsilon &&
                        Math.Abs(qTest.Y - q.Y) < epsilon &&
                        Math.Abs(qTest.Z - q.Z) < epsilon &&
                        Math.Abs(qTest.W - q.W) < epsilon);
#endif
                }
            }
Esempio n. 24
0
        /// <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)
        {
            Vector3 a, b, c;

            Matrix3x3 o;
            Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o);
            Matrix3x3.Transform(ref vA, ref o, out a);
            Matrix3x3.Transform(ref vB, ref o, out b);
            Matrix3x3.Transform(ref vC, ref o, out c);

            Vector3.Min(ref a, ref b, out boundingBox.Min);
            Vector3.Min(ref c, ref boundingBox.Min, out boundingBox.Min);

            Vector3.Max(ref a, ref b, out boundingBox.Max);
            Vector3.Max(ref c, ref boundingBox.Max, out boundingBox.Max);

            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;
        }
Esempio n. 25
0
        /// <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.
            //Notice only three directions are used.  Due to box symmetry, 'left' is just -right.
            var     direction = new Vector3(o.M11, o.M21, o.M31);
            Vector3 right;
            GetLocalExtremePointWithoutMargin(ref direction, out right);

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

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

            Matrix3x3.Transform(ref right, ref o, out right);
            Matrix3x3.Transform(ref up, ref o, out up);
            Matrix3x3.Transform(ref backward, ref o, out backward);
            //These right/up/backward represent the extreme points in world space along the world space axes.

            boundingBox.Max.X = shapeTransform.Position.X + collisionMargin + right.X;
            boundingBox.Max.Y = shapeTransform.Position.Y + collisionMargin + up.Y;
            boundingBox.Max.Z = shapeTransform.Position.Z + collisionMargin + backward.Z;

            boundingBox.Min.X = shapeTransform.Position.X - collisionMargin - right.X;
            boundingBox.Min.Y = shapeTransform.Position.Y - collisionMargin - up.Y;
            boundingBox.Min.Z = shapeTransform.Position.Z - collisionMargin - backward.Z;
        }
Esempio n. 26
0
        /// <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.
            //Notice only three directions are used.  Due to cylinder symmetry, 'left' is just -right.
            var direction = new System.Numerics.Vector3(o.M11, o.M21, o.M31);
            System.Numerics.Vector3 right;
            GetLocalExtremePointWithoutMargin(ref direction, out right);

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

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

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

            //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 - positive.X - collisionMargin;
            boundingBox.Min.Y = shapeTransform.Position.Y - positive.Y - collisionMargin;
            boundingBox.Min.Z = shapeTransform.Position.Z - positive.Z - collisionMargin;
        }
Esempio n. 27
0
        public override void Update(float dt)
        {
            //First, refresh all existing contacts.  This is an incremental manifold.
            ContactRefresher.ContactRefresh(contacts, supplementData, ref convex.worldTransform, ref triangle.worldTransform, contactIndicesToRemove);
            RemoveQueuedContacts();


            //Compute the local triangle vertices.
            //TODO: this could be quicker and cleaner.
            localTriangleShape.collisionMargin = triangle.Shape.collisionMargin;
            localTriangleShape.sidedness       = triangle.Shape.sidedness;
            Matrix3x3 orientation;

            Matrix3x3.CreateFromQuaternion(ref triangle.worldTransform.Orientation, out orientation);
            Matrix3x3.Transform(ref triangle.Shape.vA, ref orientation, out localTriangleShape.vA);
            Matrix3x3.Transform(ref triangle.Shape.vB, ref orientation, out localTriangleShape.vB);
            Matrix3x3.Transform(ref triangle.Shape.vC, ref orientation, out localTriangleShape.vC);
            Vector3.Add(ref localTriangleShape.vA, ref triangle.worldTransform.Position, out localTriangleShape.vA);
            Vector3.Add(ref localTriangleShape.vB, ref triangle.worldTransform.Position, out localTriangleShape.vB);
            Vector3.Add(ref localTriangleShape.vC, ref triangle.worldTransform.Position, out localTriangleShape.vC);

            Vector3.Subtract(ref localTriangleShape.vA, ref convex.worldTransform.Position, out localTriangleShape.vA);
            Vector3.Subtract(ref localTriangleShape.vB, ref convex.worldTransform.Position, out localTriangleShape.vB);
            Vector3.Subtract(ref localTriangleShape.vC, ref convex.worldTransform.Position, out localTriangleShape.vC);
            Matrix3x3.CreateFromQuaternion(ref convex.worldTransform.Orientation, out orientation);
            Matrix3x3.TransformTranspose(ref localTriangleShape.vA, ref orientation, out localTriangleShape.vA);
            Matrix3x3.TransformTranspose(ref localTriangleShape.vB, ref orientation, out localTriangleShape.vB);
            Matrix3x3.TransformTranspose(ref localTriangleShape.vC, ref orientation, out localTriangleShape.vC);

            //Now, generate a contact between the two shapes.
            ContactData contact;
            TinyStructList <ContactData> contactList;

            if (pairTester.GenerateContactCandidates(localTriangleShape, out contactList))
            {
                for (int i = 0; i < contactList.Count; i++)
                {
                    contactList.Get(i, out contact);
                    //Put the contact into world space.
                    Matrix3x3.Transform(ref contact.Position, ref orientation, out contact.Position);
                    Vector3.Add(ref contact.Position, ref convex.worldTransform.Position, out contact.Position);
                    Matrix3x3.Transform(ref contact.Normal, ref orientation, out contact.Normal);
                    //Check if the contact is unique before proceeding.
                    if (IsContactUnique(ref contact))
                    {
                        //Check if adding the new contact would overflow the manifold.
                        if (contacts.Count == 4)
                        {
                            //Adding that contact would overflow the manifold.  Reduce to the best subset.
                            bool addCandidate;
                            ContactReducer.ReduceContacts(contacts, ref contact, contactIndicesToRemove, out addCandidate);
                            RemoveQueuedContacts();
                            if (addCandidate)
                            {
                                Add(ref contact);
                            }
                        }
                        else
                        {
                            //Won't overflow the manifold, so just toss it in PROVIDED that it isn't too close to something else.
                            Add(ref contact);
                        }
                    }
                }
            }
            else
            {
                //Clear out the contacts, it's separated.
                for (int i = contacts.Count - 1; i >= 0; i--)
                {
                    Remove(i);
                }
            }
        }
Esempio n. 28
0
        /// <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);

            float minX, maxX;
            float minY, maxY;
            float minZ, maxZ;
            var   right    = new Vector3(o.M11, o.M21, o.M31);
            var   up       = new Vector3(o.M12, o.M22, o.M32);
            var   backward = new Vector3(o.M13, o.M23, o.M33);
            Vector3.Dot(ref vertices[0], ref right, out maxX);
            minX = maxX;
            Vector3.Dot(ref vertices[0], ref up, out maxY);
            minY = maxY;
            Vector3.Dot(ref vertices[0], ref backward, out maxZ);
            minZ = maxZ;
            int minXIndex = 0;
            int maxXIndex = 0;
            int minYIndex = 0;
            int maxYIndex = 0;
            int minZIndex = 0;
            int maxZIndex = 0;
            for (int i = 1; i < vertices.Length; ++i)
            {
                float dot;
                Vector3.Dot(ref vertices[i], ref right, out dot);
                if (dot < minX)
                {
                    minX      = dot;
                    minXIndex = i;
                }
                else if (dot > maxX)
                {
                    maxX      = dot;
                    maxXIndex = i;
                }

                Vector3.Dot(ref vertices[i], ref up, out dot);
                if (dot < minY)
                {
                    minY      = dot;
                    minYIndex = i;
                }
                else if (dot > maxY)
                {
                    maxY      = dot;
                    maxYIndex = i;
                }

                Vector3.Dot(ref vertices[i], ref backward, out dot);
                if (dot < minZ)
                {
                    minZ      = dot;
                    minZIndex = i;
                }
                else if (dot > maxZ)
                {
                    maxZ      = dot;
                    maxZIndex = i;
                }
            }

            //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 vertices[maxXIndex], ref vertices[maxYIndex], ref vertices[maxZIndex], ref o, out positive);
            TransformLocalExtremePoints(ref vertices[minXIndex], ref vertices[minYIndex], ref vertices[minZIndex], 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;
        }
Esempio n. 29
0
 ///<summary>
 /// Constructs a new affine tranform.
 ///</summary>
 ///<param name="orientation">Orientation to use as the linear transform.</param>
 ///<param name="translation">Translation to use in the transform.</param>
 public AffineTransform(ref Quaternion orientation, ref Vector3 translation)
 {
     Matrix3x3.CreateFromQuaternion(ref orientation, out LinearTransform);
     Translation = translation;
 }
Esempio n. 30
0
 /// <summary>
 /// Creates an affine transform from a rigid transform.
 /// </summary>
 /// <param name="rigid">Rigid transform to base the affine transform on.</param>
 /// <param name="affine">Affine transform created from the rigid transform.</param>
 public static void CreateFromRigidTransform(ref RigidTransform rigid, out AffineTransform affine)
 {
     affine.Translation = rigid.Position;
     Matrix3x3.CreateFromQuaternion(ref rigid.Orientation, out affine.LinearTransform);
 }