/// <summary>
/// Sets up the joint transforms by automatically creating perpendicular vectors to complete the bases.
/// </summary>
/// <param name="worldTwistAxisA">Twist axis in world space to attach to entity A.</param>
/// <param name="worldTwistAxisB">Twist axis in world space to attach to entity B.</param>
public void SetupJointTransforms(Vector3 worldTwistAxisA, Vector3 worldTwistAxisB)
{
worldTwistAxisA.Normalize();
worldTwistAxisB.Normalize();
Vector3 worldXAxis;
Vector3.Cross(ref worldTwistAxisA, ref Toolbox.UpVector, out worldXAxis);
float length = worldXAxis.LengthSquared();
if (length < Toolbox.Epsilon)
{
Vector3.Cross(ref worldTwistAxisA, ref Toolbox.RightVector, out worldXAxis);
}
worldXAxis.Normalize();
//Complete A's basis.
Vector3 worldYAxis;
Vector3.Cross(ref worldTwistAxisA, ref worldXAxis, out worldYAxis);
BasisA.rotationMatrix = connectionA.orientationMatrix;
BasisA.SetWorldAxes(worldTwistAxisA, worldXAxis, worldYAxis);
//Rotate the axis to B since it could be arbitrarily rotated.
Quaternion rotation;
Quaternion.GetQuaternionBetweenNormalizedVectors(ref worldTwistAxisA, ref worldTwistAxisB, out rotation);
Quaternion.Transform(ref worldXAxis, ref rotation, out worldXAxis);
BasisB.rotationMatrix = connectionB.orientationMatrix;
BasisB.SetWorldAxes(worldTwistAxisB, worldXAxis);
}
/// <summary>
/// Do any necessary computations to prepare the constraint for this frame.
/// </summary>
/// <param name="dt">Simulation step length.</param>
public override void Update(float dt)
{
BasisA.rotationMatrix = connectionA.orientationMatrix;
BasisB.rotationMatrix = connectionB.orientationMatrix;
BasisA.ComputeWorldSpaceAxes();
BasisB.ComputeWorldSpaceAxes();
if (Settings.mode == MotorMode.Servomechanism)
{
Quaternion rotation;
Quaternion.GetQuaternionBetweenNormalizedVectors(ref BasisB.primaryAxis, ref BasisA.primaryAxis,
out rotation);
//Transform b's 'Y' axis so that it is perpendicular with a's 'X' axis for measurement.
Vector3 twistMeasureAxis;
Quaternion.Transform(ref BasisB.xAxis, ref rotation, out twistMeasureAxis);
//By dotting the measurement vector with a 2d plane's axes, we can get a local X and Y value.
float y, x;
Vector3.Dot(ref twistMeasureAxis, ref BasisA.yAxis, out y);
Vector3.Dot(ref twistMeasureAxis, ref BasisA.xAxis, out x);
float angle = (float)Math.Atan2(y, x);
//Compute goal velocity.
Error = GetDistanceFromGoal(angle);
float absErrorOverDt = Math.Abs(Error / dt);
float errorReduction;
Settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction,
out usedSoftness);
biasVelocity = Math.Sign(Error) * MathHelper.Min(Settings.servo.baseCorrectiveSpeed, absErrorOverDt) +
Error * errorReduction;
biasVelocity = MathHelper.Clamp(biasVelocity, -Settings.servo.maxCorrectiveVelocity,
Settings.servo.maxCorrectiveVelocity);
}
else
{
biasVelocity = Settings.velocityMotor.goalVelocity;
usedSoftness = Settings.velocityMotor.softness / dt;
Error = 0;
}
//The nice thing about this approach is that the jacobian entry doesn't flip.
//Instead, the error can be negative due to the use of Atan2.
//This is important for limits which have a unique high and low value.
//Compute the jacobian.
Vector3.Add(ref BasisA.primaryAxis, ref BasisB.primaryAxis, out jacobianB);
if (jacobianB.LengthSquared() < Toolbox.Epsilon)
{
//A nasty singularity can show up if the axes are aligned perfectly.
//In a 'real' situation, this is impossible, so just ignore it.
isActiveInSolver = false;
return;
}
jacobianB.Normalize();
jacobianA.X = -jacobianB.X;
jacobianA.Y = -jacobianB.Y;
jacobianA.Z = -jacobianB.Z;
//Update the maximum force
ComputeMaxForces(Settings.maximumForce, dt);
//****** EFFECTIVE MASS MATRIX ******//
//Connection A's contribution to the mass matrix
float entryA;
Vector3 transformedAxis;
if (connectionA.isDynamic)
{
Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianA, out entryA);
}
else
{
entryA = 0;
}
//Connection B's contribution to the mass matrix
float entryB;
if (connectionB.isDynamic)
{
Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianB, out entryB);
}
else
{
entryB = 0;
}
//Compute the inverse mass matrix
velocityToImpulse = 1 / (usedSoftness + entryA + entryB);
}
