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