/// <summary>
/// Rotates the view direction up or down relative to the locked up vector.
/// </summary>
/// <param name="radians">Amount to rotate.</param>
public void Pitch(float radians)
{
//Do not allow the new view direction to violate the maximum pitch.
float dot;
Vector3.Dot(ref viewDirection, ref lockedUp, out dot);
//While this could be rephrased in terms of dot products alone, converting to actual angles can be more intuitive.
//Consider +Pi/2 to be up, and -Pi/2 to be down.
float currentPitch = (float)Math.Acos(MathHelper.Clamp(-dot, -1, 1)) - MathHelper.PiOver2;
//Compute our new pitch by clamping the current + change.
float newPitch = MathHelper.Clamp(currentPitch + radians, -maximumPitch, maximumPitch);
float allowedChange = newPitch - currentPitch;
//Compute and apply the rotation.
Vector3 pitchAxis;
Vector3.Cross(ref viewDirection, ref lockedUp, out pitchAxis);
//This is guaranteed safe by all interaction points stopping viewDirection from being aligned with lockedUp.
pitchAxis.Normalize();
Matrix3x3 rotation;
Matrix3x3.CreateFromAxisAngle(ref pitchAxis, allowedChange, out rotation);
Matrix3x3.Transform(ref viewDirection, ref rotation, out viewDirection);
//Avoid drift by renormalizing.
viewDirection.Normalize();
}
/// <summary>
/// Rotates the camera around its locked up vector.
/// </summary>
/// <param name="radians">Amount to rotate.</param>
public void Yaw(float radians)
{
//Rotate around the up vector.
Matrix3x3 rotation;
Matrix3x3.CreateFromAxisAngle(ref lockedUp, radians, out rotation);
Matrix3x3.Transform(ref viewDirection, ref rotation, out viewDirection);
//Avoid drift by renormalizing.
viewDirection.Normalize();
}
///<summary>
/// Performs the frame's configuration step.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//Transform the axes into world space.
basis.rotationMatrix = connectionA.orientationMatrix;
basis.ComputeWorldSpaceAxes();
Matrix3x3.Transform(ref localTestAxis, ref connectionB.orientationMatrix, out worldTestAxis);
//Compute the plane normals.
Vector3 minPlaneNormal, maxPlaneNormal;
//Rotate basisA y axis around the basisA primary axis.
Matrix3x3 rotation;
Matrix3x3.CreateFromAxisAngle(ref basis.primaryAxis, minimumAngle + MathHelper.PiOver2, out rotation);
Matrix3x3.Transform(ref Basis.xAxis, ref rotation, out minPlaneNormal);
Matrix3x3.CreateFromAxisAngle(ref basis.primaryAxis, maximumAngle - MathHelper.PiOver2, out rotation);
Matrix3x3.Transform(ref Basis.xAxis, ref rotation, out maxPlaneNormal);
//Compute the errors along the two normals.
float planePositionMin, planePositionMax;
Vector3.Dot(ref minPlaneNormal, ref worldTestAxis, out planePositionMin);
Vector3.Dot(ref maxPlaneNormal, ref worldTestAxis, out planePositionMax);
float span = GetDistanceFromMinimum(maximumAngle);
//Early out and compute the determine the plane normal.
if (span >= MathHelper.Pi)
{
if (planePositionMax > 0 || planePositionMin > 0)
{
//It's in a perfectly valid configuration, so skip.
isActiveInSolver = false;
minIsActive = false;
maxIsActive = false;
error = Vector2.Zero;
accumulatedImpulse = Vector2.Zero;
isLimitActive = false;
return;
}
if (planePositionMax > planePositionMin)
{
//It's quicker to escape out to the max plane than the min plane.
error.X = 0;
error.Y = -planePositionMax;
accumulatedImpulse.X = 0;
minIsActive = false;
maxIsActive = true;
}
else
{
//It's quicker to escape out to the min plane than the max plane.
error.X = -planePositionMin;
error.Y = 0;
accumulatedImpulse.Y = 0;
minIsActive = true;
maxIsActive = false;
}
//There's never a non-degenerate situation where having both planes active with a span
//greater than pi is useful.
}
else
{
if (planePositionMax > 0 && planePositionMin > 0)
{
//It's in a perfectly valid configuration, so skip.
isActiveInSolver = false;
minIsActive = false;
maxIsActive = false;
error = Vector2.Zero;
accumulatedImpulse = Vector2.Zero;
isLimitActive = false;
return;
}
if (planePositionMin <= 0 && planePositionMax <= 0)
{
//Escape upward.
//Activate both planes.
error.X = -planePositionMin;
error.Y = -planePositionMax;
minIsActive = true;
maxIsActive = true;
}
else if (planePositionMin <= 0)
{
//It's quicker to escape out to the min plane than the max plane.
error.X = -planePositionMin;
error.Y = 0;
accumulatedImpulse.Y = 0;
minIsActive = true;
maxIsActive = false;
}
else
{
//It's quicker to escape out to the max plane than the min plane.
error.X = 0;
error.Y = -planePositionMax;
accumulatedImpulse.X = 0;
minIsActive = false;
maxIsActive = true;
}
}
isLimitActive = true;
//****** VELOCITY BIAS ******//
//Compute the correction velocity
float errorReduction;
springSettings.ComputeErrorReductionAndSoftness(dt, 1 / dt, out errorReduction, out softness);
//Compute the jacobians
if (minIsActive)
{
Vector3.Cross(ref minPlaneNormal, ref worldTestAxis, out jacobianMinA);
if (jacobianMinA.LengthSquared() < Toolbox.Epsilon)
{
//The plane normal is aligned with the test axis.
//Use the basis's free axis.
jacobianMinA = basis.primaryAxis;
}
jacobianMinA.Normalize();
jacobianMinB.X = -jacobianMinA.X;
jacobianMinB.Y = -jacobianMinA.Y;
jacobianMinB.Z = -jacobianMinA.Z;
}
if (maxIsActive)
{
Vector3.Cross(ref maxPlaneNormal, ref worldTestAxis, out jacobianMaxA);
if (jacobianMaxA.LengthSquared() < Toolbox.Epsilon)
{
//The plane normal is aligned with the test axis.
//Use the basis's free axis.
jacobianMaxA = basis.primaryAxis;
}
jacobianMaxA.Normalize();
jacobianMaxB.X = -jacobianMaxA.X;
jacobianMaxB.Y = -jacobianMaxA.Y;
jacobianMaxB.Z = -jacobianMaxA.Z;
}
//Error is always positive
if (minIsActive)
{
biasVelocity.X = MathHelper.Min(MathHelper.Max(0, error.X - margin) * errorReduction, maxCorrectiveVelocity);
if (bounciness > 0)
{
float relativeVelocity;
float dot;
//Find the velocity contribution from each connection
Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMinA, out relativeVelocity);
Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMinB, out dot);
relativeVelocity += dot;
biasVelocity.X = MathHelper.Max(biasVelocity.X, ComputeBounceVelocity(-relativeVelocity));
}
}
if (maxIsActive)
{
biasVelocity.Y = MathHelper.Min(MathHelper.Max(0, error.Y - margin) * errorReduction, maxCorrectiveVelocity);
if (bounciness > 0)
{
//Find the velocity contribution from each connection
if (maxIsActive)
{
float relativeVelocity;
Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMaxA, out relativeVelocity);
float dot;
Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMaxB, out dot);
relativeVelocity += dot;
biasVelocity.Y = MathHelper.Max(biasVelocity.Y, ComputeBounceVelocity(-relativeVelocity));
}
}
}
//****** EFFECTIVE MASS MATRIX ******//
//Connection A's contribution to the mass matrix
float minEntryA, minEntryB;
float maxEntryA, maxEntryB;
Vector3 transformedAxis;
if (connectionA.isDynamic)
{
if (minIsActive)
{
Matrix3x3.Transform(ref jacobianMinA, ref connectionA.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianMinA, out minEntryA);
}
else
{
minEntryA = 0;
}
if (maxIsActive)
{
Matrix3x3.Transform(ref jacobianMaxA, ref connectionA.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianMaxA, out maxEntryA);
}
else
{
maxEntryA = 0;
}
}
else
{
minEntryA = 0;
maxEntryA = 0;
}
//Connection B's contribution to the mass matrix
if (connectionB.isDynamic)
{
if (minIsActive)
{
Matrix3x3.Transform(ref jacobianMinB, ref connectionB.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianMinB, out minEntryB);
}
else
{
minEntryB = 0;
}
if (maxIsActive)
{
Matrix3x3.Transform(ref jacobianMaxB, ref connectionB.inertiaTensorInverse, out transformedAxis);
Vector3.Dot(ref transformedAxis, ref jacobianMaxB, out maxEntryB);
}
else
{
maxEntryB = 0;
}
}
else
{
minEntryB = 0;
maxEntryB = 0;
}
//Compute the inverse mass matrix
//Notice that the mass matrix isn't linked, it's two separate ones.
velocityToImpulse.X = 1 / (softness + minEntryA + minEntryB);
velocityToImpulse.Y = 1 / (softness + maxEntryA + maxEntryB);
}