/// <summary>
/// Gets the bounding box of an element in the data.
/// </summary>
/// <param name="triangleIndex">Index of the triangle in the data.</param>
/// <param name="boundingBox">Bounding box of the triangle.</param>
public void GetBoundingBox(int triangleIndex, out BoundingBox boundingBox)
{
Vector3f v1, v2, v3;
GetTriangle(triangleIndex, out v1, out v2, out v3);
Vector3f.ComponentMin(ref v1, ref v2, out boundingBox.Min);
Vector3f.ComponentMin(ref boundingBox.Min, ref v3, out boundingBox.Min);
Vector3f.ComponentMax(ref v1, ref v2, out boundingBox.Max);
Vector3f.ComponentMax(ref boundingBox.Max, ref v3, out boundingBox.Max);
}
/// <summary>
/// Initializes the detector entity and any other necessary logic.
/// </summary>
protected internal override void Initialize()
{
//Setup the dimensions of the detector.
Vector3f startpoint = wheel.suspension.localAttachmentPoint;
Vector3f endpoint = startpoint + wheel.suspension.localDirection * wheel.suspension.restLength;
Vector3f min, max;
Vector3f.ComponentMin(ref startpoint, ref endpoint, out min);
Vector3f.ComponentMax(ref startpoint, ref endpoint, out max);
detector.Width = max.X - min.X;
detector.Height = max.Y - min.Y;
detector.Length = max.Z - min.Z;
}
/// <summary>
/// Gets the bounding box of the shape given a transform.
/// </summary>
/// <param name="shapeTransform">Transform to use.</param>
/// <param name="boundingBox">Bounding box of the transformed shape.</param>
public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox)
{
Vector3f a, b, c;
Matrix3f o;
Matrix3f.FromQuaternion(ref shapeTransform.Orientation, out o);
Vector3f.Transform(ref vA, ref o, out a);
Vector3f.Transform(ref vB, ref o, out b);
Vector3f.Transform(ref vC, ref o, out c);
Vector3f.ComponentMin(ref a, ref b, out boundingBox.Min);
Vector3f.ComponentMin(ref c, ref boundingBox.Min, out boundingBox.Min);
Vector3f.ComponentMax(ref a, ref b, out boundingBox.Max);
Vector3f.ComponentMax(ref c, ref boundingBox.Max, out boundingBox.Max);
boundingBox.Min.X += shapeTransform.Position.X - collisionMargin;
boundingBox.Min.Y += shapeTransform.Position.Y - collisionMargin;
boundingBox.Min.Z += shapeTransform.Position.Z - collisionMargin;
boundingBox.Max.X += shapeTransform.Position.X + collisionMargin;
boundingBox.Max.Y += shapeTransform.Position.Y + collisionMargin;
boundingBox.Max.Z += shapeTransform.Position.Z + collisionMargin;
}
protected internal override void SolveVelocityIteration()
{
//Compute the 'relative' linear and angular velocities. For single bone constraints, it's based entirely on the one bone's velocities!
//They have to be pulled into constraint space first to compute the necessary impulse, though.
Vector3f linearContributionA;
Vector3f.TransformTranspose(ref ConnectionA.linearVelocity, ref linearJacobianA, out linearContributionA);
Vector3f angularContributionA;
Vector3f.TransformTranspose(ref ConnectionA.angularVelocity, ref angularJacobianA, out angularContributionA);
Vector3f linearContributionB;
Vector3f.TransformTranspose(ref ConnectionB.linearVelocity, ref linearJacobianB, out linearContributionB);
Vector3f angularContributionB;
Vector3f.TransformTranspose(ref ConnectionB.angularVelocity, ref angularJacobianB, out angularContributionB);
//The constraint velocity error will be the velocity we try to remove.
Vector3f constraintVelocityError;
Vector3f.Add(ref linearContributionA, ref angularContributionA, out constraintVelocityError);
Vector3f.Add(ref constraintVelocityError, ref linearContributionB, out constraintVelocityError);
Vector3f.Add(ref constraintVelocityError, ref angularContributionB, out constraintVelocityError);
//However, we need to take into account two extra sources of velocities which modify our target velocity away from zero.
//First, the velocity bias from position correction:
Vector3f.Subtract(ref constraintVelocityError, ref velocityBias, out constraintVelocityError);
//And second, the bias from softness:
Vector3f softnessBias;
Vector3f.Multiply(ref accumulatedImpulse, -softness, out softnessBias);
Vector3f.Subtract(ref constraintVelocityError, ref softnessBias, out constraintVelocityError);
//By now, the constraint velocity error contains all the velocity we want to get rid of.
//Convert it into an impulse using the effective mass matrix.
Vector3f constraintSpaceImpulse;
Vector3f.Transform(ref constraintVelocityError, ref effectiveMass, out constraintSpaceImpulse);
Vector3f.Negate(ref constraintSpaceImpulse, out constraintSpaceImpulse);
//Add the constraint space impulse to the accumulated impulse so that warm starting and softness work properly.
Vector3f preadd = accumulatedImpulse;
Vector3f.Add(ref constraintSpaceImpulse, ref accumulatedImpulse, out accumulatedImpulse);
//Limits can only apply positive impulses.
Vector3f.ComponentMax(ref Toolbox.ZeroVector, ref accumulatedImpulse, out accumulatedImpulse);
//But wait! The accumulated impulse may exceed this constraint's capacity! Check to make sure!
float impulseSquared = accumulatedImpulse.LengthSquared;
if (impulseSquared > maximumImpulseSquared)
{
//Oops! Clamp that down.
Vector3f.Multiply(ref accumulatedImpulse, maximumImpulse / (float)Math.Sqrt(impulseSquared), out accumulatedImpulse);
}
//Update the impulse based upon the clamped accumulated impulse and the original, pre-add accumulated impulse.
Vector3f.Subtract(ref accumulatedImpulse, ref preadd, out constraintSpaceImpulse);
//The constraint space impulse now represents the impulse we want to apply to the bone... but in constraint space.
//Bring it out to world space using the transposed jacobian.
if (!ConnectionA.Pinned)//Treat pinned elements as if they have infinite inertia.
{
Vector3f linearImpulseA;
Vector3f.Transform(ref constraintSpaceImpulse, ref linearJacobianA, out linearImpulseA);
Vector3f angularImpulseA;
Vector3f.Transform(ref constraintSpaceImpulse, ref angularJacobianA, out angularImpulseA);
//Apply them!
ConnectionA.ApplyLinearImpulse(ref linearImpulseA);
ConnectionA.ApplyAngularImpulse(ref angularImpulseA);
}
if (!ConnectionB.Pinned)//Treat pinned elements as if they have infinite inertia.
{
Vector3f linearImpulseB;
Vector3f.Transform(ref constraintSpaceImpulse, ref linearJacobianB, out linearImpulseB);
Vector3f angularImpulseB;
Vector3f.Transform(ref constraintSpaceImpulse, ref angularJacobianB, out angularImpulseB);
//Apply them!
ConnectionB.ApplyLinearImpulse(ref linearImpulseB);
ConnectionB.ApplyAngularImpulse(ref angularImpulseB);
}
}
