protected internal override void UpdateJacobiansAndVelocityBias()
{
linearJacobianA = linearJacobianB = new Matrix3x3();
angularJacobianA = new Matrix3x3 {
M11 = 1, M22 = 1, M33 = 1
};
angularJacobianB = new Matrix3x3 {
M11 = -1, M22 = -1, M33 = -1
};
//The error is computed using this equation:
//GoalRelativeOrientation * ConnectionA.Orientation * Error = ConnectionB.Orientation
//GoalRelativeOrientation is the original rotation from A to B in A's local space.
//Multiplying by A's orientation gives us where B *should* be.
//Of course, B won't be exactly where it should be after initialization.
//The Error component holds the difference between what is and what should be.
//Error = (GoalRelativeOrientation * ConnectionA.Orientation)^-1 * ConnectionB.Orientation
System.Numerics.Quaternion bTarget;
QuaternionEx.Concatenate(ref GoalRelativeOrientation, ref ConnectionA.Orientation, out bTarget);
System.Numerics.Quaternion bTargetConjugate;
QuaternionEx.Conjugate(ref bTarget, out bTargetConjugate);
System.Numerics.Quaternion error;
QuaternionEx.Concatenate(ref bTargetConjugate, ref ConnectionB.Orientation, out error);
//Convert the error into an axis-angle vector usable for bias velocity.
float angle;
System.Numerics.Vector3 axis;
QuaternionEx.GetAxisAngleFromQuaternion(ref error, out axis, out angle);
velocityBias.X = errorCorrectionFactor * axis.X * angle;
velocityBias.Y = errorCorrectionFactor * axis.Y * angle;
velocityBias.Z = errorCorrectionFactor * axis.Z * angle;
}
///<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;
}
/// <summary>
/// Constructs a 3DOF angular joint which tries to keep two bones in angular alignment.
/// </summary>
/// <param name="connectionA">First bone to connect to the joint.</param>
/// <param name="connectionB">Second bone to connect to the joint.</param>
public IKAngularJoint(Bone connectionA, Bone connectionB)
: base(connectionA, connectionB)
{
System.Numerics.Quaternion orientationAConjugate;
QuaternionEx.Conjugate(ref ConnectionA.Orientation, out orientationAConjugate);
//Store the orientation from A to B in A's local space in the GoalRelativeOrientation.
QuaternionEx.Concatenate(ref ConnectionB.Orientation, ref orientationAConjugate, out GoalRelativeOrientation);
}
///<summary>
/// Gets the local transform of B in the space of A.
///</summary>
///<param name="transformA">First transform.</param>
///<param name="transformB">Second transform.</param>
///<param name="localTransformB">Transform of B in the local space of A.</param>
public static void GetLocalTransform(ref RigidTransform transformA, ref RigidTransform transformB,
out RigidTransform localTransformB)
{
//Put B into A's space.
System.Numerics.Quaternion conjugateOrientationA;
QuaternionEx.Conjugate(ref transformA.Orientation, out conjugateOrientationA);
QuaternionEx.Concatenate(ref transformB.Orientation, ref conjugateOrientationA, out localTransformB.Orientation);
Vector3Ex.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position);
QuaternionEx.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position);
}
/// <summary>
/// Constructs a new constraint which attempts to restrict the relative angular motion of two entities.
/// </summary>
/// <param name="connectionA">First connection of the pair.</param>
/// <param name="connectionB">Second connection of the pair.</param>
public AngularMotor(Entity connectionA, Entity connectionB)
{
ConnectionA = connectionA;
ConnectionB = connectionB;
settings = new MotorSettingsOrientation(this);
//Compute the rotation from A to B in A's local space.
System.Numerics.Quaternion orientationAConjugate;
QuaternionEx.Conjugate(ref connectionA.orientation, out orientationAConjugate);
QuaternionEx.Concatenate(ref connectionB.orientation, ref orientationAConjugate, out settings.servo.goal);
}
void CreateRingPlatform(Vector3 position, Box ringBoxShape, BodyDescription bodyDescription, float radius)
{
var innerCircumference = MathF.PI * 2 * (radius - ringBoxShape.HalfLength);
var boxCount = (int)(0.95f * innerCircumference / ringBoxShape.Height);
float increment = MathHelper.TwoPi / boxCount;
for (int i = 0; i < boxCount; i++)
{
var angle = i * increment;
bodyDescription.Pose = new RigidPose(
position + new Vector3(-MathF.Cos(angle) * radius, ringBoxShape.HalfWidth, MathF.Sin(angle) * radius),
QuaternionEx.Concatenate(QuaternionEx.CreateFromAxisAngle(Vector3.UnitZ, MathF.PI * 0.5f), QuaternionEx.CreateFromAxisAngle(Vector3.UnitY, angle + MathF.PI * 0.5f)));
Simulation.Bodies.Add(bodyDescription);
}
}
///<summary>
/// Sweeps two shapes against another.
///</summary>
///<param name="shapeA">First shape being swept.</param>
///<param name="shapeB">Second shape being swept.</param>
///<param name="sweepA">Sweep vector for the first shape.</param>
///<param name="sweepB">Sweep vector for the second shape.</param>
///<param name="transformA">Transform to apply to the first shape.</param>
///<param name="transformB">Transform to apply to the second shape.</param>
///<param name="hit">Hit data of the sweep test, if any.</param>
///<returns>Whether or not the swept shapes hit each other..</returns>
public static bool ConvexCast(ConvexShape shapeA, ConvexShape shapeB, ref System.Numerics.Vector3 sweepA, ref System.Numerics.Vector3 sweepB, ref RigidTransform transformA, ref RigidTransform transformB,
out RayHit hit)
{
//Put the velocity into shapeA's local space.
System.Numerics.Vector3 velocityWorld;
Vector3Ex.Subtract(ref sweepB, ref sweepA, out velocityWorld);
System.Numerics.Quaternion conjugateOrientationA;
QuaternionEx.Conjugate(ref transformA.Orientation, out conjugateOrientationA);
System.Numerics.Vector3 rayDirection;
QuaternionEx.Transform(ref velocityWorld, ref conjugateOrientationA, out rayDirection);
//Transform b into a's local space.
RigidTransform localTransformB;
QuaternionEx.Concatenate(ref transformB.Orientation, ref conjugateOrientationA, out localTransformB.Orientation);
Vector3Ex.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position);
QuaternionEx.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position);
System.Numerics.Vector3 w, p;
hit.T = 0;
hit.Location = System.Numerics.Vector3.Zero; //The ray starts at the origin.
hit.Normal = Toolbox.ZeroVector;
System.Numerics.Vector3 v = hit.Location;
RaySimplex simplex = new RaySimplex();
float vw, vdir;
int count = 0;
do
{
if (++count > MaximumGJKIterations)
{
//It's taken too long to find a hit. Numerical problems are probable; quit.
hit = new RayHit();
return(false);
}
MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref v, ref localTransformB, out p);
Vector3Ex.Subtract(ref hit.Location, ref p, out w);
Vector3Ex.Dot(ref v, ref w, out vw);
if (vw > 0)
{
Vector3Ex.Dot(ref v, ref rayDirection, out vdir);
if (vdir >= 0)
{
hit = new RayHit();
return(false);
}
hit.T = hit.T - vw / vdir;
if (hit.T > 1)
{
//If we've gone beyond where the ray can reach, there's obviously no hit.
hit = new RayHit();
return(false);
}
//Shift the ray up.
Vector3Ex.Multiply(ref rayDirection, hit.T, out hit.Location);
//The ray origin is the origin! Don't need to add any ray position.
hit.Normal = v;
}
RaySimplex shiftedSimplex;
simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex);
shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v);
//Could measure the progress of the ray. If it's too little, could early out.
//Not used by default since it's biased towards precision over performance.
} while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref Toolbox.ZeroVector));
//This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident.
//Transform the hit data into world space.
QuaternionEx.Transform(ref hit.Normal, ref transformA.Orientation, out hit.Normal);
Vector3Ex.Multiply(ref velocityWorld, hit.T, out hit.Location);
Vector3Ex.Add(ref hit.Location, ref transformA.Position, out hit.Location);
return(true);
}
/// <summary>
/// Initializes the constraint for the current frame.
/// </summary>
/// <param name="dt">Time between frames.</param>
public override void Update(float dt)
{
basis.rotationMatrix = connectionA.orientationMatrix;
basis.ComputeWorldSpaceAxes();
if (settings.mode == MotorMode.Servomechanism) //Only need to do the bulk of this work if it's a servo.
{
//The error is computed using this equation:
//GoalRelativeOrientation * ConnectionA.Orientation * Error = ConnectionB.Orientation
//GoalRelativeOrientation is the original rotation from A to B in A's local space.
//Multiplying by A's orientation gives us where B *should* be.
//Of course, B won't be exactly where it should be after initialization.
//The Error component holds the difference between what is and what should be.
//Error = (GoalRelativeOrientation * ConnectionA.Orientation)^-1 * ConnectionB.Orientation
//ConnectionA.Orientation is replaced in the above by the world space basis orientation.
System.Numerics.Quaternion worldBasis = QuaternionEx.CreateFromRotationMatrix(basis.WorldTransform);
System.Numerics.Quaternion bTarget;
QuaternionEx.Concatenate(ref settings.servo.goal, ref worldBasis, out bTarget);
System.Numerics.Quaternion bTargetConjugate;
QuaternionEx.Conjugate(ref bTarget, out bTargetConjugate);
System.Numerics.Quaternion error;
QuaternionEx.Concatenate(ref bTargetConjugate, ref connectionB.orientation, out error);
float errorReduction;
settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out usedSoftness);
//Turn this into an axis-angle representation.
QuaternionEx.GetAxisAngleFromQuaternion(ref error, out axis, out angle);
//Scale the axis by the desired velocity if the angle is sufficiently large (epsilon).
if (angle > Toolbox.BigEpsilon)
{
float velocity = -(MathHelper.Min(settings.servo.baseCorrectiveSpeed, angle / dt) + angle * errorReduction);
biasVelocity.X = axis.X * velocity;
biasVelocity.Y = axis.Y * velocity;
biasVelocity.Z = axis.Z * velocity;
//Ensure that the corrective velocity doesn't exceed the max.
float length = biasVelocity.LengthSquared();
if (length > settings.servo.maxCorrectiveVelocitySquared)
{
float multiplier = settings.servo.maxCorrectiveVelocity / (float)Math.Sqrt(length);
biasVelocity.X *= multiplier;
biasVelocity.Y *= multiplier;
biasVelocity.Z *= multiplier;
}
}
else
{
biasVelocity.X = 0;
biasVelocity.Y = 0;
biasVelocity.Z = 0;
}
}
else
{
usedSoftness = settings.velocityMotor.softness / dt;
angle = 0; //Zero out the error;
Matrix3x3 transform = basis.WorldTransform;
Matrix3x3.Transform(ref settings.velocityMotor.goalVelocity, ref transform, out biasVelocity);
}
//Compute effective mass
Matrix3x3.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out effectiveMassMatrix);
effectiveMassMatrix.M11 += usedSoftness;
effectiveMassMatrix.M22 += usedSoftness;
effectiveMassMatrix.M33 += usedSoftness;
Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix);
//Update the maximum force
ComputeMaxForces(settings.maximumForce, dt);
}
/// <summary>
/// Finds a supporting entity, the contact location, and the contact normal.
/// </summary>
/// <param name="location">Contact point between the wheel and the support.</param>
/// <param name="normal">Contact normal between the wheel and the support.</param>
/// <param name="suspensionLength">Length of the suspension at the contact.</param>
/// <param name="supportingCollidable">Collidable supporting the wheel, if any.</param>
/// <param name="entity">Supporting object.</param>
/// <param name="material">Material of the wheel.</param>
/// <returns>Whether or not any support was found.</returns>
protected internal override bool FindSupport(out System.Numerics.Vector3 location, out System.Numerics.Vector3 normal, out float suspensionLength, out Collidable supportingCollidable, out Entity entity, out Material material)
{
suspensionLength = float.MaxValue;
location = Toolbox.NoVector;
supportingCollidable = null;
entity = null;
normal = Toolbox.NoVector;
material = null;
Collidable testCollidable;
RayHit rayHit;
bool hit = false;
System.Numerics.Quaternion localSteeringTransform;
QuaternionEx.CreateFromAxisAngle(ref wheel.suspension.localDirection, steeringAngle, out localSteeringTransform);
var startingTransform = new RigidTransform
{
Position = wheel.suspension.worldAttachmentPoint,
Orientation = QuaternionEx.Concatenate(QuaternionEx.Concatenate(LocalWheelOrientation, IncludeSteeringTransformInCast ? localSteeringTransform : System.Numerics.Quaternion.Identity), wheel.vehicle.Body.orientation)
};
System.Numerics.Vector3 sweep;
Vector3Ex.Multiply(ref wheel.suspension.worldDirection, wheel.suspension.restLength, out sweep);
for (int i = 0; i < detector.CollisionInformation.pairs.Count; i++)
{
var pair = detector.CollisionInformation.pairs[i];
testCollidable = (pair.BroadPhaseOverlap.entryA == detector.CollisionInformation ? pair.BroadPhaseOverlap.entryB : pair.BroadPhaseOverlap.entryA) as Collidable;
if (testCollidable != null)
{
if (CollisionRules.CollisionRuleCalculator(this, testCollidable) == CollisionRule.Normal &&
testCollidable.ConvexCast(shape, ref startingTransform, ref sweep, out rayHit) &&
rayHit.T * wheel.suspension.restLength < suspensionLength)
{
suspensionLength = rayHit.T * wheel.suspension.restLength;
EntityCollidable entityCollidable;
if ((entityCollidable = testCollidable as EntityCollidable) != null)
{
entity = entityCollidable.Entity;
material = entityCollidable.Entity.Material;
}
else
{
entity = null;
supportingCollidable = testCollidable;
var materialOwner = testCollidable as IMaterialOwner;
if (materialOwner != null)
{
material = materialOwner.Material;
}
}
location = rayHit.Location;
normal = rayHit.Normal;
hit = true;
}
}
}
if (hit)
{
if (suspensionLength > 0)
{
float dot;
Vector3Ex.Dot(ref normal, ref wheel.suspension.worldDirection, out dot);
if (dot > 0)
{
//The cylinder cast produced a normal which is opposite of what we expect.
Vector3Ex.Negate(ref normal, out normal);
}
normal.Normalize();
}
else
{
Vector3Ex.Negate(ref wheel.suspension.worldDirection, out normal);
}
return(true);
}
return(false);
}
