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