protected internal override void UpdateJacobiansAndVelocityBias() { linearJacobianA = Matrix3x3.Identity; //The jacobian entries are is [ La, Aa, -Lb, -Ab ] because the relative velocity is computed using A-B. So, negate B's jacobians! linearJacobianB = new Matrix3x3 { M11 = -1, M22 = -1, M33 = -1 }; System.Numerics.Vector3 rA; QuaternionEx.Transform(ref LocalOffsetA, ref ConnectionA.Orientation, out rA); Matrix3x3.CreateCrossProduct(ref rA, out angularJacobianA); //Transposing a skew-symmetric matrix is equivalent to negating it. Matrix3x3.Transpose(ref angularJacobianA, out angularJacobianA); System.Numerics.Vector3 worldPositionA; Vector3Ex.Add(ref ConnectionA.Position, ref rA, out worldPositionA); System.Numerics.Vector3 rB; QuaternionEx.Transform(ref LocalOffsetB, ref ConnectionB.Orientation, out rB); Matrix3x3.CreateCrossProduct(ref rB, out angularJacobianB); System.Numerics.Vector3 worldPositionB; Vector3Ex.Add(ref ConnectionB.Position, ref rB, out worldPositionB); System.Numerics.Vector3 linearError; Vector3Ex.Subtract(ref worldPositionB, ref worldPositionA, out linearError); Vector3Ex.Multiply(ref linearError, errorCorrectionFactor, out velocityBias); }
/// <summary> /// Integrates the position and orientation of the bone forward based upon the current linear and angular velocity. /// </summary> internal void UpdatePosition() { //Update the position based on the linear velocity. Vector3Ex.Add(ref Position, ref linearVelocity, out Position); //Update the orientation based on the angular velocity. System.Numerics.Vector3 increment; Vector3Ex.Multiply(ref angularVelocity, .5f, out increment); var multiplier = new System.Numerics.Quaternion(increment.X, increment.Y, increment.Z, 0); QuaternionEx.Multiply(ref multiplier, ref Orientation, out multiplier); QuaternionEx.Add(ref Orientation, ref multiplier, out Orientation); Orientation = System.Numerics.Quaternion.Normalize(Orientation); //Eliminate any latent velocity in the bone to prevent unwanted simulation feedback. //This is the only thing conceptually separating this "IK" solver from the regular dynamics loop in BEPUphysics. //(Well, that and the whole lack of collision detection...) linearVelocity = new System.Numerics.Vector3(); angularVelocity = new System.Numerics.Vector3(); //Note: Unlike a regular dynamics simulation, we do not include any 'dt' parameter in the above integration. //Setting the velocity to 0 every update means that no more than a single iteration's worth of velocity accumulates. //Since the softness of constraints already varies with the time step and bones never accelerate for more than one frame, //scaling the velocity for position integration actually turns out generally worse. //This is not a rigorously justifiable approach, but this isn't a regular dynamic simulation anyway. }
static void SplitReposition(Entity a, Entity b, ref ShapeDistributionInformation distributionInfoA, ref ShapeDistributionInformation distributionInfoB, float weightA, float weightB) { //The compounds are not aligned with the original's position yet. //In order to align them, first look at the centers the split method computed. //They are offsets from the center of the original shape in local space. //These can be used to reposition the objects in world space. System.Numerics.Vector3 weightedA, weightedB; Vector3Ex.Multiply(ref distributionInfoA.Center, weightA, out weightedA); Vector3Ex.Multiply(ref distributionInfoB.Center, weightB, out weightedB); System.Numerics.Vector3 newLocalCenter; Vector3Ex.Add(ref weightedA, ref weightedB, out newLocalCenter); Vector3Ex.Divide(ref newLocalCenter, weightA + weightB, out newLocalCenter); System.Numerics.Vector3 localOffsetA; System.Numerics.Vector3 localOffsetB; Vector3Ex.Subtract(ref distributionInfoA.Center, ref newLocalCenter, out localOffsetA); Vector3Ex.Subtract(ref distributionInfoB.Center, ref newLocalCenter, out localOffsetB); System.Numerics.Vector3 originalPosition = a.position; b.Orientation = a.Orientation; System.Numerics.Vector3 offsetA = QuaternionEx.Transform(localOffsetA, a.Orientation); System.Numerics.Vector3 offsetB = QuaternionEx.Transform(localOffsetB, a.Orientation); a.Position = originalPosition + offsetA; b.Position = originalPosition + offsetB; System.Numerics.Vector3 originalLinearVelocity = a.linearVelocity; System.Numerics.Vector3 originalAngularVelocity = a.angularVelocity; a.AngularVelocity = originalAngularVelocity; b.AngularVelocity = originalAngularVelocity; a.LinearVelocity = originalLinearVelocity + System.Numerics.Vector3.Cross(originalAngularVelocity, offsetA); b.LinearVelocity = originalLinearVelocity + System.Numerics.Vector3.Cross(originalAngularVelocity, offsetB); }
/// <summary> /// Updates the movement basis of the horizontal motion constraint. /// Should be updated automatically by the character on each time step; other code should not need to call this. /// </summary> /// <param name="forward">Forward facing direction of the character.</param> public void UpdateMovementBasis(ref System.Numerics.Vector3 forward) { System.Numerics.Vector3 down = characterBody.orientationMatrix.Down; System.Numerics.Vector3 strafeDirection; System.Numerics.Vector3 horizontalForwardDirection = forward - down * Vector3Ex.Dot(down, forward); float forwardLengthSquared = horizontalForwardDirection.LengthSquared(); if (forwardLengthSquared < Toolbox.Epsilon) { //Use an arbitrary direction to complete the basis. horizontalForwardDirection = characterBody.orientationMatrix.Forward; strafeDirection = characterBody.orientationMatrix.Right; } else { Vector3Ex.Divide(ref horizontalForwardDirection, (float)Math.Sqrt(forwardLengthSquared), out horizontalForwardDirection); Vector3Ex.Cross(ref down, ref horizontalForwardDirection, out strafeDirection); //Don't need to normalize the strafe direction; it's the cross product of two normalized perpendicular vectors. } Vector3Ex.Multiply(ref horizontalForwardDirection, movementDirection.Y, out movementDirection3d); System.Numerics.Vector3 strafeComponent; Vector3Ex.Multiply(ref strafeDirection, movementDirection.X, out strafeComponent); Vector3Ex.Add(ref strafeComponent, ref movementDirection3d, out movementDirection3d); }
protected internal override void GetContactInformation(int index, out ContactInformation info) { info.Contact = MeshManifold.contacts.Elements[index]; //Find the contact's normal and friction forces. info.FrictionImpulse = 0; info.NormalImpulse = 0; for (int i = 0; i < contactConstraint.frictionConstraints.Count; i++) { if (contactConstraint.frictionConstraints.Elements[i].PenetrationConstraint.contact == info.Contact) { info.FrictionImpulse = contactConstraint.frictionConstraints.Elements[i].accumulatedImpulse; info.NormalImpulse = contactConstraint.frictionConstraints.Elements[i].PenetrationConstraint.accumulatedImpulse; break; } } //Compute relative velocity if (convex.entity != null) { System.Numerics.Vector3 velocity; Vector3Ex.Subtract(ref info.Contact.Position, ref convex.entity.position, out velocity); Vector3Ex.Cross(ref convex.entity.angularVelocity, ref velocity, out velocity); Vector3Ex.Add(ref velocity, ref convex.entity.linearVelocity, out info.RelativeVelocity); } else { info.RelativeVelocity = new System.Numerics.Vector3(); } info.Pair = this; }
///<summary> /// Gets the closest point on the segment to the origin. ///</summary> ///<param name="point">Closest point.</param> public void GetPointOnSegmentClosestToOrigin(out System.Numerics.Vector3 point) { System.Numerics.Vector3 segmentDisplacement; Vector3Ex.Subtract(ref B, ref A, out segmentDisplacement); float dotA; Vector3Ex.Dot(ref segmentDisplacement, ref A, out dotA); //Inside segment. float V = -dotA / segmentDisplacement.LengthSquared(); Vector3Ex.Multiply(ref segmentDisplacement, V, out point); Vector3Ex.Add(ref point, ref A, out point); //if (dotB > 0) //{ //} //else //{ // //It is not possible to be anywhere but within the segment in a 'boolean' GJK, where it early outs as soon as a separating axis is found. // //Outside B. // //Remove current A; we're becoming a point. // A = B; // State = SimplexState.Point; // point = A; //} //It can never be outside A! //That would mean that the origin is LESS extreme along the search direction than our extreme point--- our search direction would not have picked that direction. }
static void RemoveReposition(Entity compound, ref ShapeDistributionInformation distributionInfo, float weight, float removedWeight, ref System.Numerics.Vector3 removedCenter) { //The compounds are not aligned with the original's position yet. //In order to align them, first look at the centers the split method computed. //They are offsets from the center of the original shape in local space. //These can be used to reposition the objects in world space. System.Numerics.Vector3 weightedA, weightedB; Vector3Ex.Multiply(ref distributionInfo.Center, weight, out weightedA); Vector3Ex.Multiply(ref removedCenter, removedWeight, out weightedB); System.Numerics.Vector3 newLocalCenter; Vector3Ex.Add(ref weightedA, ref weightedB, out newLocalCenter); Vector3Ex.Divide(ref newLocalCenter, weight + removedWeight, out newLocalCenter); System.Numerics.Vector3 localOffset; Vector3Ex.Subtract(ref distributionInfo.Center, ref newLocalCenter, out localOffset); System.Numerics.Vector3 originalPosition = compound.position; System.Numerics.Vector3 offset = QuaternionEx.Transform(localOffset, compound.orientation); compound.Position = originalPosition + offset; System.Numerics.Vector3 originalLinearVelocity = compound.linearVelocity; System.Numerics.Vector3 originalAngularVelocity = compound.angularVelocity; compound.AngularVelocity = originalAngularVelocity; compound.LinearVelocity = originalLinearVelocity + System.Numerics.Vector3.Cross(originalAngularVelocity, offset); }
/// <summary> /// Applies the corrective impulses required by the constraint. /// </summary> public override float SolveIteration() { System.Numerics.Vector3 velocityDifference; Vector3Ex.Subtract(ref connectionB.angularVelocity, ref connectionA.angularVelocity, out velocityDifference); System.Numerics.Vector3 softnessVector; Vector3Ex.Multiply(ref accumulatedImpulse, softness, out softnessVector); System.Numerics.Vector3 lambda; Vector3Ex.Add(ref velocityDifference, ref biasVelocity, out lambda); Vector3Ex.Subtract(ref lambda, ref softnessVector, out lambda); Matrix3x3.Transform(ref lambda, ref effectiveMassMatrix, out lambda); Vector3Ex.Add(ref lambda, ref accumulatedImpulse, out accumulatedImpulse); if (connectionA.isDynamic) { connectionA.ApplyAngularImpulse(ref lambda); } if (connectionB.isDynamic) { System.Numerics.Vector3 torqueB; Vector3Ex.Negate(ref lambda, out torqueB); connectionB.ApplyAngularImpulse(ref torqueB); } return(Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z)); }
/// <summary> /// Changes the relative velocity between the character and its support. /// </summary> /// <param name="supportData">Support data to use to jump.</param> /// <param name="velocityChange">Change to apply to the character and support relative velocity.</param> /// <param name="relativeVelocity">Relative velocity to update.</param> void ApplyJumpVelocity(ref SupportData supportData, System.Numerics.Vector3 velocityChange, ref System.Numerics.Vector3 relativeVelocity) { Body.LinearVelocity += velocityChange; var entityCollidable = supportData.SupportObject as EntityCollidable; if (entityCollidable != null) { if (entityCollidable.Entity.IsDynamic) { System.Numerics.Vector3 change = velocityChange * jumpForceFactor; //Multiple characters cannot attempt to modify another entity's velocity at the same time. entityCollidable.Entity.Locker.Enter(); try { entityCollidable.Entity.LinearMomentum += change * -Body.Mass; } finally { entityCollidable.Entity.Locker.Exit(); } velocityChange += change; } } //Update the relative velocity as well. It's a ref parameter, so this update will be reflected in the calling scope. Vector3Ex.Add(ref relativeVelocity, ref velocityChange, out relativeVelocity); }
void ICCDPositionUpdateable.UpdatePositionContinuously(float dt) { float minimumToi = 1; for (int i = 0; i < collisionInformation.pairs.Count; i++) { if (collisionInformation.pairs.Elements[i].timeOfImpact < minimumToi) { minimumToi = collisionInformation.pairs.Elements[i].timeOfImpact; } } //The orientation was already updated by the PreUpdatePosition. //However, to be here, this object is not a discretely updated object. //That means we still need to update the linear motion. System.Numerics.Vector3 increment; Vector3Ex.Multiply(ref linearVelocity, dt * minimumToi, out increment); Vector3Ex.Add(ref position, ref increment, out position); collisionInformation.UpdateWorldTransform(ref position, ref orientation); if (PositionUpdated != null) { PositionUpdated(this); } MathChecker.Validate(linearVelocity); MathChecker.Validate(angularVelocity); MathChecker.Validate(position); MathChecker.Validate(orientation); #if CONSERVE MathChecker.Validate(angularMomentum); #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) { #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 System.Numerics.Vector3(Math.Sign(o.M11) * halfWidth, Math.Sign(o.M21) * halfHeight, Math.Sign(o.M31) * halfLength); var up = new System.Numerics.Vector3(Math.Sign(o.M12) * halfWidth, Math.Sign(o.M22) * halfHeight, Math.Sign(o.M32) * halfLength); var backward = new System.Numerics.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. System.Numerics.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. Vector3Ex.Add(ref shapeTransform.Position, ref offset, out boundingBox.Max); Vector3Ex.Subtract(ref shapeTransform.Position, ref offset, out boundingBox.Min); }
void IPositionUpdateable.PreUpdatePosition(float dt) { System.Numerics.Vector3 increment; Vector3Ex.Multiply(ref angularVelocity, dt * .5f, out increment); var multiplier = new System.Numerics.Quaternion(increment.X, increment.Y, increment.Z, 0); QuaternionEx.Multiply(ref multiplier, ref orientation, out multiplier); QuaternionEx.Add(ref orientation, ref multiplier, out orientation); orientation = System.Numerics.Quaternion.Normalize(orientation); Matrix3x3.CreateFromQuaternion(ref orientation, out orientationMatrix); //Only do the linear motion if this object doesn't obey CCD. if (PositionUpdateMode == PositionUpdateMode.Discrete) { Vector3Ex.Multiply(ref linearVelocity, dt, out increment); Vector3Ex.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> /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// </summary> public override void ExclusiveUpdate() { //****** WARM STARTING ******// //Apply accumulated impulse if (connectionA.isDynamic) { var impulse = new System.Numerics.Vector3(); if (minIsActive) { Vector3Ex.Multiply(ref jacobianMinA, accumulatedImpulse.X, out impulse); } if (maxIsActive) { System.Numerics.Vector3 temp; Vector3Ex.Multiply(ref jacobianMaxA, accumulatedImpulse.Y, out temp); Vector3Ex.Add(ref impulse, ref temp, out impulse); } connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { var impulse = new System.Numerics.Vector3(); if (minIsActive) { Vector3Ex.Multiply(ref jacobianMinB, accumulatedImpulse.X, out impulse); } if (maxIsActive) { System.Numerics.Vector3 temp; Vector3Ex.Multiply(ref jacobianMaxB, accumulatedImpulse.Y, out temp); Vector3Ex.Add(ref impulse, ref temp, out impulse); } connectionB.ApplyAngularImpulse(ref impulse); } }
protected override void ConfigureCollidable(TriangleEntry entry, float 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); System.Numerics.Vector3 center; Vector3Ex.Add(ref shape.vA, ref shape.vB, out center); Vector3Ex.Add(ref center, ref shape.vC, out center); Vector3Ex.Multiply(ref center, 1 / 3f, out center); Vector3Ex.Subtract(ref shape.vA, ref center, out shape.vA); Vector3Ex.Subtract(ref shape.vB, ref center, out shape.vB); Vector3Ex.Subtract(ref shape.vC, ref center, out shape.vC); Vector3Ex.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 = System.Numerics.Quaternion.Identity; entry.Collidable.UpdateBoundingBoxInternal(dt); }
protected override TriangleCollidable GetOpposingCollidable(int index) { //Construct a TriangleCollidable from the static mesh. var toReturn = PhysicsResources.GetTriangleCollidable(); var shape = toReturn.Shape; mesh.Shape.TriangleMesh.Data.GetTriangle(index, out shape.vA, out shape.vB, out shape.vC); Matrix3x3.Transform(ref shape.vA, ref mesh.worldTransform.LinearTransform, out shape.vA); Matrix3x3.Transform(ref shape.vB, ref mesh.worldTransform.LinearTransform, out shape.vB); Matrix3x3.Transform(ref shape.vC, ref mesh.worldTransform.LinearTransform, out shape.vC); System.Numerics.Vector3 center; Vector3Ex.Add(ref shape.vA, ref shape.vB, out center); Vector3Ex.Add(ref center, ref shape.vC, out center); Vector3Ex.Multiply(ref center, 1 / 3f, out center); Vector3Ex.Subtract(ref shape.vA, ref center, out shape.vA); Vector3Ex.Subtract(ref shape.vB, ref center, out shape.vB); Vector3Ex.Subtract(ref shape.vC, ref center, out shape.vC); Vector3Ex.Add(ref center, ref mesh.worldTransform.Translation, out center); //The bounding box doesn't update by itself. toReturn.worldTransform.Position = center; toReturn.worldTransform.Orientation = System.Numerics.Quaternion.Identity; toReturn.UpdateBoundingBoxInternal(0); shape.sidedness = mesh.Sidedness; shape.collisionMargin = mobileMesh.Shape.MeshCollisionMargin; return(toReturn); }
///<summary> /// Concatenates a rigid transform with another rigid transform. ///</summary> ///<param name="a">The first rigid transform.</param> ///<param name="b">The second rigid transform.</param> ///<param name="combined">Concatenated rigid transform.</param> public static void Multiply(ref RigidTransform a, ref RigidTransform b, out RigidTransform combined) { System.Numerics.Vector3 intermediate; QuaternionEx.Transform(ref a.Position, ref b.Orientation, out intermediate); Vector3Ex.Add(ref intermediate, ref b.Position, out combined.Position); QuaternionEx.Concatenate(ref a.Orientation, ref b.Orientation, out combined.Orientation); }
/// <summary> /// Updates the position of the detector before each step. /// </summary> protected internal override void UpdateDetectorPosition() { #if !WINDOWS System.Numerics.Vector3 newPosition = new System.Numerics.Vector3(); #else System.Numerics.Vector3 newPosition; #endif newPosition.X = wheel.suspension.worldAttachmentPoint.X + wheel.suspension.worldDirection.X * wheel.suspension.restLength * .5f; newPosition.Y = wheel.suspension.worldAttachmentPoint.Y + wheel.suspension.worldDirection.Y * wheel.suspension.restLength * .5f; newPosition.Z = wheel.suspension.worldAttachmentPoint.Z + wheel.suspension.worldDirection.Z * wheel.suspension.restLength * .5f; detector.Position = newPosition; if (IncludeSteeringTransformInCast) { System.Numerics.Quaternion localSteeringTransform; QuaternionEx.CreateFromAxisAngle(ref wheel.suspension.localDirection, steeringAngle, out localSteeringTransform); detector.Orientation = QuaternionEx.Concatenate(localSteeringTransform, wheel.Vehicle.Body.orientation); } else { detector.Orientation = wheel.Vehicle.Body.orientation; } System.Numerics.Vector3 linearVelocity; Vector3Ex.Subtract(ref newPosition, ref wheel.vehicle.Body.position, out linearVelocity); Vector3Ex.Cross(ref linearVelocity, ref wheel.vehicle.Body.angularVelocity, out linearVelocity); Vector3Ex.Add(ref linearVelocity, ref wheel.vehicle.Body.linearVelocity, out linearVelocity); detector.LinearVelocity = linearVelocity; detector.AngularVelocity = wheel.vehicle.Body.angularVelocity; }
void IForceUpdateable.UpdateForForces(float dt) { //Linear velocity if (IsAffectedByGravity) { Vector3Ex.Add(ref forceUpdater.gravityDt, ref linearVelocity, out linearVelocity); } //Boost damping at very low velocities. This is a strong stabilizer; removes a ton of energy from the system. if (activityInformation.DeactivationManager.useStabilization && activityInformation.allowStabilization && (activityInformation.isSlowing || activityInformation.velocityTimeBelowLimit > activityInformation.DeactivationManager.lowVelocityTimeMinimum)) { float energy = linearVelocity.LengthSquared() + angularVelocity.LengthSquared(); if (energy < activityInformation.DeactivationManager.velocityLowerLimitSquared) { float boost = 1 - (float)(Math.Sqrt(energy) / (2f * activityInformation.DeactivationManager.velocityLowerLimit)); ModifyAngularDamping(boost); ModifyLinearDamping(boost); } } //Damping float linear = LinearDamping + linearDampingBoost; if (linear > 0) { Vector3Ex.Multiply(ref linearVelocity, (float)Math.Pow(MathHelper.Clamp(1 - linear, 0, 1), dt), out linearVelocity); } //When applying angular damping, the momentum or velocity is damped depending on the conservation setting. float angular = AngularDamping + angularDampingBoost; if (angular > 0) { #if CONSERVE Vector3Ex.Multiply(ref angularMomentum, (float)Math.Pow(MathHelper.Clamp(1 - angular, 0, 1), dt), out angularMomentum); #else Vector3Ex.Multiply(ref angularVelocity, (float)Math.Pow(MathHelper.Clamp(1 - angular, 0, 1), dt), out angularVelocity); #endif } linearDampingBoost = 0; angularDampingBoost = 0; //Update world inertia tensors. Matrix3x3 multiplied; Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensorInverse, out multiplied); Matrix3x3.Multiply(ref multiplied, ref orientationMatrix, out inertiaTensorInverse); Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensor, out multiplied); Matrix3x3.Multiply(ref multiplied, ref orientationMatrix, out inertiaTensor); #if CONSERVE //Update angular velocity. //Note that this doesn't play nice with singular inertia tensors. //Locked tensors result in zero angular velocity. Matrix3x3.Transform(ref angularMomentum, ref inertiaTensorInverse, out angularVelocity); MathChecker.Validate(angularMomentum); #endif MathChecker.Validate(linearVelocity); MathChecker.Validate(angularVelocity); }
///<summary> /// Updates the world transform of the shape using the given position and orientation. /// The world transform of the shape is offset from the given position and orientation by the collidable's LocalPosition. ///</summary> ///<param name="position">Position to use for the calculation.</param> ///<param name="orientation">Orientation to use for the calculation.</param> public virtual void UpdateWorldTransform(ref System.Numerics.Vector3 position, ref System.Numerics.Quaternion orientation) { QuaternionEx.Transform(ref localPosition, ref orientation, out worldTransform.Position); Vector3Ex.Add(ref worldTransform.Position, ref position, out worldTransform.Position); worldTransform.Orientation = orientation; worldTransform.Validate(); }
/// <summary> /// Casts a convex shape against the collidable. /// </summary> /// <param name="castShape">Shape to cast.</param> /// <param name="startingTransform">Initial transform of the shape.</param> /// <param name="sweep">Sweep to apply to the shape.</param> /// <param name="hit">Hit data, if any.</param> /// <returns>Whether or not the cast hit anything.</returns> public override bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref System.Numerics.Vector3 sweep, out RayHit hit) { hit = new RayHit(); BoundingBox boundingBox; castShape.GetSweptLocalBoundingBox(ref startingTransform, ref worldTransform, ref sweep, out boundingBox); var tri = PhysicsThreadResources.GetTriangle(); var hitElements = CommonResources.GetIntList(); if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements)) { hit.T = float.MaxValue; for (int i = 0; i < hitElements.Count; i++) { Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC); AffineTransform.Transform(ref tri.vA, ref worldTransform, out tri.vA); AffineTransform.Transform(ref tri.vB, ref worldTransform, out tri.vB); AffineTransform.Transform(ref tri.vC, ref worldTransform, out tri.vC); System.Numerics.Vector3 center; Vector3Ex.Add(ref tri.vA, ref tri.vB, out center); Vector3Ex.Add(ref center, ref tri.vC, out center); Vector3Ex.Multiply(ref center, 1f / 3f, out center); Vector3Ex.Subtract(ref tri.vA, ref center, out tri.vA); Vector3Ex.Subtract(ref tri.vB, ref center, out tri.vB); Vector3Ex.Subtract(ref tri.vC, ref center, out tri.vC); tri.MaximumRadius = tri.vA.LengthSquared(); float radius = tri.vB.LengthSquared(); if (tri.MaximumRadius < radius) { tri.MaximumRadius = radius; } radius = tri.vC.LengthSquared(); if (tri.MaximumRadius < radius) { tri.MaximumRadius = radius; } tri.MaximumRadius = (float)Math.Sqrt(tri.MaximumRadius); tri.collisionMargin = 0; var triangleTransform = new RigidTransform { Orientation = System.Numerics.Quaternion.Identity, Position = center }; RayHit tempHit; if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T) { hit = tempHit; } } tri.MaximumRadius = 0; PhysicsThreadResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return(hit.T != float.MaxValue); } PhysicsThreadResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return(false); }
/// <summary> /// Multiplies a transform by another transform. /// </summary> /// <param name="a">First transform.</param> /// <param name="b">Second transform.</param> /// <param name="transform">Combined transform.</param> public static void Multiply(ref AffineTransform a, ref AffineTransform b, out AffineTransform transform) { Matrix3x3 linearTransform;//Have to use temporary variable just in case a or b reference is transform. Matrix3x3.Multiply(ref a.LinearTransform, ref b.LinearTransform, out linearTransform); System.Numerics.Vector3 translation; Matrix3x3.Transform(ref a.Translation, ref b.LinearTransform, out translation); Vector3Ex.Add(ref translation, ref b.Translation, out transform.Translation); transform.LinearTransform = linearTransform; }
///<summary> /// Gets the extreme point of the shape in world space in a given direction. ///</summary> ///<param name="direction">Direction to find the extreme point in.</param> /// <param name="shapeTransform">Transform to use for the shape.</param> ///<param name="extremePoint">Extreme point on the shape.</param> public void GetExtremePointWithoutMargin(System.Numerics.Vector3 direction, ref RigidTransform shapeTransform, out System.Numerics.Vector3 extremePoint) { System.Numerics.Quaternion conjugate; QuaternionEx.Conjugate(ref shapeTransform.Orientation, out conjugate); QuaternionEx.Transform(ref direction, ref conjugate, out direction); GetLocalExtremePointWithoutMargin(ref direction, out extremePoint); QuaternionEx.Transform(ref extremePoint, ref shapeTransform.Orientation, out extremePoint); Vector3Ex.Add(ref extremePoint, ref shapeTransform.Position, out extremePoint); }
/// <summary> /// Calculates and applies corrective impulses. /// Called automatically by space. /// </summary> public override float SolveIteration() { #if !WINDOWS System.Numerics.Vector3 lambda = new System.Numerics.Vector3(); #else System.Numerics.Vector3 lambda; #endif //Velocity along the length. System.Numerics.Vector3 cross; System.Numerics.Vector3 aVel, bVel; Vector3Ex.Cross(ref connectionA.angularVelocity, ref worldOffsetA, out cross); Vector3Ex.Add(ref connectionA.linearVelocity, ref cross, out aVel); Vector3Ex.Cross(ref connectionB.angularVelocity, ref worldOffsetB, out cross); Vector3Ex.Add(ref connectionB.linearVelocity, ref cross, out bVel); lambda.X = aVel.X - bVel.X + biasVelocity.X - softness * accumulatedImpulse.X; lambda.Y = aVel.Y - bVel.Y + biasVelocity.Y - softness * accumulatedImpulse.Y; lambda.Z = aVel.Z - bVel.Z + biasVelocity.Z - softness * accumulatedImpulse.Z; //Turn the velocity into an impulse. Matrix3x3.Transform(ref lambda, ref massMatrix, out lambda); //ACcumulate the impulse Vector3Ex.Add(ref accumulatedImpulse, ref lambda, out accumulatedImpulse); //Apply the impulse //Constraint.applyImpulse(myConnectionA, myConnectionB, ref rA, ref rB, ref impulse); #if !WINDOWS System.Numerics.Vector3 linear = new System.Numerics.Vector3(); #else System.Numerics.Vector3 linear; #endif if (connectionA.isDynamic) { linear.X = -lambda.X; linear.Y = -lambda.Y; linear.Z = -lambda.Z; connectionA.ApplyLinearImpulse(ref linear); System.Numerics.Vector3 taImpulse; Vector3Ex.Cross(ref worldOffsetA, ref linear, out taImpulse); connectionA.ApplyAngularImpulse(ref taImpulse); } if (connectionB.isDynamic) { connectionB.ApplyLinearImpulse(ref lambda); System.Numerics.Vector3 tbImpulse; Vector3Ex.Cross(ref worldOffsetB, ref lambda, out tbImpulse); connectionB.ApplyAngularImpulse(ref tbImpulse); } return(Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z)); }
public override void UpdateCollision(float dt) { WasContaining = Containing; WasTouching = Touching; var transform = new RigidTransform { Orientation = System.Numerics.Quaternion.Identity }; DetectorVolume.TriangleMesh.Tree.GetOverlaps(convex.boundingBox, overlaps); for (int i = 0; i < overlaps.Count; i++) { DetectorVolume.TriangleMesh.Data.GetTriangle(overlaps.Elements[i], out triangle.vA, out triangle.vB, out triangle.vC); Vector3Ex.Add(ref triangle.vA, ref triangle.vB, out transform.Position); Vector3Ex.Add(ref triangle.vC, ref transform.Position, out transform.Position); Vector3Ex.Multiply(ref transform.Position, 1 / 3f, out transform.Position); Vector3Ex.Subtract(ref triangle.vA, ref transform.Position, out triangle.vA); Vector3Ex.Subtract(ref triangle.vB, ref transform.Position, out triangle.vB); Vector3Ex.Subtract(ref triangle.vC, ref transform.Position, out triangle.vC); //If this triangle collides with the convex, we can stop immediately since we know we're touching and not containing.))) //[MPR is used here in lieu of GJK because the MPR implementation tends to finish quicker when objects are overlapping than GJK. The GJK implementation does better on separated objects.] if (MPRToolbox.AreShapesOverlapping(convex.Shape, triangle, ref convex.worldTransform, ref transform)) { Touching = true; //The convex can't be fully contained if it's still touching the surface. Containing = false; overlaps.Clear(); goto events; } } overlaps.Clear(); //If we get here, then there was no shell intersection. //If the convex's center point is contained by the mesh, then the convex is fully contained. //If this is a child pair, the CheckContainment flag may be set to false. This is because the parent has //already determined that it is not contained (another child performed the check and found that it was not contained) //and that it is already touching somehow (either by intersection or by containment). //so further containment tests are unnecessary. if (CheckContainment && DetectorVolume.IsPointContained(ref convex.worldTransform.Position, overlaps)) { Touching = true; Containing = true; goto events; } //If we get here, then there was no surface intersection and the convex's center is not contained- the volume and convex are separate! Touching = false; Containing = false; events: NotifyDetectorVolumeOfChanges(); }
///<summary> /// 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); Vector3Ex.Add(ref boundingBox.Max, ref shapeTransform.Position, out boundingBox.Max); Vector3Ex.Add(ref boundingBox.Min, ref shapeTransform.Position, out boundingBox.Min); }
///<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> /// Refreshes the contact manifold, removing any out of date contacts /// and updating others. /// </summary> public static void ContactRefresh(RawList <Contact> contacts, RawValueList <ContactSupplementData> supplementData, ref RigidTransform transformA, ref RigidTransform transformB, RawList <int> toRemove) { //TODO: Could also refresh normals with some trickery. //Would also need to refresh depth using new normals, and would require some extra information. for (int k = 0; k < contacts.Count; k++) { contacts.Elements[k].Validate(); ContactSupplementData data = supplementData.Elements[k]; System.Numerics.Vector3 newPosA, newPosB; RigidTransform.Transform(ref data.LocalOffsetA, ref transformA, out newPosA); RigidTransform.Transform(ref data.LocalOffsetB, ref transformB, out newPosB); //ab - (ab*n)*n //Compute the horizontal offset. System.Numerics.Vector3 ab; Vector3Ex.Subtract(ref newPosB, ref newPosA, out ab); float dot; Vector3Ex.Dot(ref ab, ref contacts.Elements[k].Normal, out dot); System.Numerics.Vector3 temp; Vector3Ex.Multiply(ref contacts.Elements[k].Normal, dot, out temp); Vector3Ex.Subtract(ref ab, ref temp, out temp); dot = temp.LengthSquared(); if (dot > CollisionDetectionSettings.ContactInvalidationLengthSquared) { toRemove.Add(k); } else { //Depth refresh: //Find deviation ((Ra-Rb)*N) and add to base depth. Vector3Ex.Dot(ref ab, ref contacts.Elements[k].Normal, out dot); contacts.Elements[k].PenetrationDepth = data.BasePenetrationDepth - dot; if (contacts.Elements[k].PenetrationDepth < -CollisionDetectionSettings.maximumContactDistance) { toRemove.Add(k); } else { //Refresh position and ra/rb. System.Numerics.Vector3 newPos; Vector3Ex.Add(ref newPosB, ref newPosA, out newPos); Vector3Ex.Multiply(ref newPos, .5f, out newPos); contacts.Elements[k].Position = newPos; //This is an interesting idea, but has very little effect one way or the other. //data.BasePenetrationDepth = contacts.Elements[k].PenetrationDepth; //RigidTransform.TransformByInverse(ref newPos, ref transformA, out data.LocalOffsetA); //RigidTransform.TransformByInverse(ref newPos, ref transformB, out data.LocalOffsetB); } contacts.Elements[k].Validate(); } } }
void AddLocalContact(ref ContactData contact, ref Matrix3x3 orientation, ref QuickList <ContactData> candidatesToAdd) { //Put the contact into world space. Matrix3x3.Transform(ref contact.Position, ref orientation, out contact.Position); Vector3Ex.Add(ref contact.Position, ref convex.worldTransform.Position, out contact.Position); Matrix3x3.Transform(ref contact.Normal, ref orientation, out contact.Normal); //Check to see if the contact is unique before proceeding. if (IsContactUnique(ref contact, ref candidatesToAdd)) { candidatesToAdd.Add(ref contact); } }
protected internal override void UpdateJacobiansAndVelocityBias() { //Transform the anchors and offsets into world space. System.Numerics.Vector3 offsetA, offsetB; QuaternionEx.Transform(ref LocalAnchorA, ref ConnectionA.Orientation, out offsetA); QuaternionEx.Transform(ref LocalAnchorB, ref ConnectionB.Orientation, out offsetB); System.Numerics.Vector3 anchorA, anchorB; Vector3Ex.Add(ref ConnectionA.Position, ref offsetA, out anchorA); Vector3Ex.Add(ref ConnectionB.Position, ref offsetB, out anchorB); //Compute the distance. System.Numerics.Vector3 separation; Vector3Ex.Subtract(ref anchorB, ref anchorA, out separation); float currentDistance = separation.Length(); //Compute jacobians System.Numerics.Vector3 linearA; #if !WINDOWS linearA = new System.Numerics.Vector3(); #endif if (currentDistance > Toolbox.Epsilon) { linearA.X = separation.X / currentDistance; linearA.Y = separation.Y / currentDistance; linearA.Z = separation.Z / currentDistance; velocityBias = new System.Numerics.Vector3(errorCorrectionFactor * (currentDistance - distance), 0, 0); } else { velocityBias = new System.Numerics.Vector3(); linearA = new System.Numerics.Vector3(); } System.Numerics.Vector3 angularA, angularB; Vector3Ex.Cross(ref offsetA, ref linearA, out angularA); //linearB = -linearA, so just swap the cross product order. Vector3Ex.Cross(ref linearA, ref offsetB, out angularB); //Put all the 1x3 jacobians into a 3x3 matrix representation. linearJacobianA = new Matrix3x3 { M11 = linearA.X, M12 = linearA.Y, M13 = linearA.Z }; linearJacobianB = new Matrix3x3 { M11 = -linearA.X, M12 = -linearA.Y, M13 = -linearA.Z }; angularJacobianA = new Matrix3x3 { M11 = angularA.X, M12 = angularA.Y, M13 = angularA.Z }; angularJacobianB = new Matrix3x3 { M11 = angularB.X, M12 = angularB.Y, M13 = angularB.Z }; }
///<summary> /// Gets the extreme point of the shape in world space in a given direction with margin expansion. ///</summary> ///<param name="direction">Direction to find the extreme point in.</param> /// <param name="shapeTransform">Transform to use for the shape.</param> ///<param name="extremePoint">Extreme point on the shape.</param> public void GetExtremePoint(System.Numerics.Vector3 direction, ref RigidTransform shapeTransform, out System.Numerics.Vector3 extremePoint) { GetExtremePointWithoutMargin(direction, ref shapeTransform, out extremePoint); float directionLength = direction.LengthSquared(); if (directionLength > Toolbox.Epsilon) { Vector3Ex.Multiply(ref direction, collisionMargin / (float)Math.Sqrt(directionLength), out direction); Vector3Ex.Add(ref extremePoint, ref direction, out extremePoint); } }