/// <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(Fix64 radians)
{
//Do not allow the new view direction to violate the maximum pitch.
Fix64 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.
Fix64 currentPitch = Fix64.Acos(MathHelper.Clamp(-dot, -1, 1)) - MathHelper.PiOver2;
//Compute our new pitch by clamping the current + change.
Fix64 newPitch = MathHelper.Clamp(currentPitch + radians, -maximumPitch, maximumPitch);
Fix64 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>
/// Computes the axis angle representation of a normalized quaternion.
/// </summary>
/// <param name="q">Quaternion to be converted.</param>
/// <param name="axis">Axis represented by the quaternion.</param>
/// <param name="angle">Angle around the axis represented by the quaternion.</param>
public static void GetAxisAngleFromQuaternion(ref Quaternion q, out Vector3 axis, out Fix64 angle)
{
#if !WINDOWS
axis = new Vector3();
#endif
Fix64 qw = q.W;
if (qw > F64.C0)
{
axis.X = q.X;
axis.Y = q.Y;
axis.Z = q.Z;
}
else
{
axis.X = -q.X;
axis.Y = -q.Y;
axis.Z = -q.Z;
qw = -qw;
}
Fix64 lengthSquared = axis.LengthSquared();
if (lengthSquared > F64.C1em14)
{
Vector3.Divide(ref axis, Fix64.Sqrt(lengthSquared), out axis);
angle = F64.C2 * Fix64.Acos(MathHelper.Clamp(qw, -1, F64.C1));
}
else
{
axis = Toolbox.UpVector;
angle = F64.C0;
}
}
/// <summary>
/// Computes the angle change represented by a normalized quaternion.
/// </summary>
/// <param name="q">Quaternion to be converted.</param>
/// <returns>Angle around the axis represented by the quaternion.</returns>
public static Fix64 GetAngleFromQuaternion(ref Quaternion q)
{
Fix64 qw = Fix64.Abs(q.W);
if (qw > F64.C1)
{
return(F64.C0);
}
return(F64.C2 * Fix64.Acos(qw));
}
public static Fix64 Angle(FixQuaternion a, FixQuaternion b)
{
FixQuaternion aInv = FixQuaternion.Inverse(a);
FixQuaternion f = b * aInv;
Fix64 angle = Fix64.Acos(f.w) * 2 * Fix64.Rad2Deg;
if (angle > 180)
{
angle = 360 - angle;
}
return(angle);
}
/// <summary>
/// Updates the upright constraint.
/// Called automatically by its owning space.
/// </summary>
/// <param name="dt">Time since last frame in simulation seconds.</param>
void IDuringForcesUpdateable.Update(Fix64 dt)
{
myWorldUpVector = Matrix3x3.Transform(myLocalUpVector, Entity.OrientationMatrix);
//Compute the axis and angle
Vector3 axis = Vector3.Cross(myWorldUpVector, Vector3.Up);
var angle = Fix64.Acos(Vector3.Dot(Vector3.Up, myWorldUpVector));
if (angle > MinimumAngle && angle < MaximumAngle)
{
angle = angle - MinimumAngle;
axis.Normalize();
Entity.AngularMomentum += (axis * (angle * CorrectionFactor * dt));
}
}
public static FixQuaternion Slerp(FixQuaternion from, FixQuaternion to, Fix64 t)
{
t = FixMath.Clamp(t, 0, 1);
Fix64 dot = Dot(from, to);
if (dot < 0.0f)
{
to = Multiply(to, -1);
dot = -dot;
}
Fix64 halfTheta = Fix64.Acos(dot);
return(Multiply(Multiply(from, Fix64.Sin((1 - t) * halfTheta)) + Multiply(to, Fix64.Sin(t * halfTheta)), 1 / Fix64.Sin(halfTheta)));
}
public Fix64 GetAngleFromX()
{
if (sqrtMagnitude <= (Fix64)0)
{
return((Fix64)0);
}
Fix64 cosA = x / magnitude;
Fix64 sinA = y / magnitude;
if (sinA == (Fix64)0 && cosA < -Fix64.One / (Fix64)2)
{
return((Fix64)Fix64.Pi);
}
Fix64 temAngle = (Fix64)Fix64.Sign(sinA) * (Fix64)Fix64.Acos(cosA);
return(temAngle);
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
linearJacobianA = linearJacobianB = new Matrix3x3();
//There are two free axes and one restricted axis.
//The constraint attempts to keep the hinge axis attached to connection A and the twist axis attached to connection B perpendicular to each other.
//The restricted axis is the cross product between the twist and hinge axes.
Vector3 worldTwistAxis, worldHingeAxis;
Quaternion.Transform(ref LocalHingeAxis, ref ConnectionA.Orientation, out worldHingeAxis);
Quaternion.Transform(ref LocalTwistAxis, ref ConnectionB.Orientation, out worldTwistAxis);
Vector3 restrictedAxis;
Vector3.Cross(ref worldHingeAxis, ref worldTwistAxis, out restrictedAxis);
//Attempt to normalize the restricted axis.
Fix64 lengthSquared = restrictedAxis.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
Vector3.Divide(ref restrictedAxis, Fix64.Sqrt(lengthSquared), out restrictedAxis);
}
else
{
restrictedAxis = new Vector3();
}
angularJacobianA = new Matrix3x3
{
M11 = restrictedAxis.X,
M12 = restrictedAxis.Y,
M13 = restrictedAxis.Z,
};
Matrix3x3.Negate(ref angularJacobianA, out angularJacobianB);
Fix64 error;
Vector3.Dot(ref worldHingeAxis, ref worldTwistAxis, out error);
error = Fix64.Acos(MathHelper.Clamp(error, -1, F64.C1)) - MathHelper.PiOver2;
velocityBias = new Vector3(errorCorrectionFactor * error, F64.C0, F64.C0);
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
//This constraint doesn't consider linear motion.
linearJacobianA = linearJacobianB = new Matrix3x3();
//Compute the world axes.
Vector3 axisA, axisB;
Quaternion.Transform(ref LocalAxisA, ref ConnectionA.Orientation, out axisA);
Quaternion.Transform(ref LocalAxisB, ref ConnectionB.Orientation, out axisB);
Fix64 dot;
Vector3.Dot(ref axisA, ref axisB, out dot);
//Yes, we could avoid this acos here. Performance is not the highest goal of this system; the less tricks used, the easier it is to understand.
// TODO investigate performance
Fix64 angle = Fix64.Acos(MathHelper.Clamp(dot, -1, F64.C1));
//One angular DOF is constrained by this limit.
Vector3 hingeAxis;
Vector3.Cross(ref axisA, ref axisB, out hingeAxis);
angularJacobianA = new Matrix3x3 {
M11 = hingeAxis.X, M12 = hingeAxis.Y, M13 = hingeAxis.Z
};
angularJacobianB = new Matrix3x3 {
M11 = -hingeAxis.X, M12 = -hingeAxis.Y, M13 = -hingeAxis.Z
};
//Note how we've computed the jacobians despite the limit being potentially inactive.
//This is to enable 'speculative' limits.
if (angle >= maximumAngle)
{
velocityBias = new Vector3(errorCorrectionFactor * (angle - maximumAngle), F64.C0, F64.C0);
}
else
{
//The constraint is not yet violated. But, it may be- allow only as much motion as could occur without violating the constraint.
//Limits can't 'pull,' so this will not result in erroneous sticking.
velocityBias = new Vector3(angle - maximumAngle, F64.C0, F64.C0);
}
}
public static Fix64 Angle(FractionalHex origin, FractionalHex direction, FractionalHex x, bool inRadians = true)
{
var diference = x - origin;
direction = direction.Normalized();
var adjacent = DotProduct(direction, diference);
var hipotenuse = diference.Magnitude();
var cos = hipotenuse != Fix64.Zero? adjacent / hipotenuse: Fix64.One;
cos = cos.Clamp(Fix64.One, -Fix64.One);
if (inRadians)
{
return(Fix64.Acos(cos));
}
else
{
return(Fix64.Acos(Fix64.Degrees(cos)));
}
}
/// <summary>
/// Blends two quaternions together to get an intermediate state.
/// </summary>
/// <param name="start">Starting point of the interpolation.</param>
/// <param name="end">Ending point of the interpolation.</param>
/// <param name="interpolationAmount">Amount of the end point to use.</param>
/// <param name="result">Interpolated intermediate quaternion.</param>
public static void Slerp(ref Quaternion start, ref Quaternion end, Fix64 interpolationAmount, out Quaternion result)
{
Fix64 cosHalfTheta = start.W * end.W + start.X * end.X + start.Y * end.Y + start.Z * end.Z;
if (cosHalfTheta < F64.C0)
{
//Negating a quaternion results in the same orientation,
//but we need cosHalfTheta to be positive to get the shortest path.
end.X = -end.X;
end.Y = -end.Y;
end.Z = -end.Z;
end.W = -end.W;
cosHalfTheta = -cosHalfTheta;
}
// If the orientations are similar enough, then just pick one of the inputs.
if (cosHalfTheta > F64.C1m1em12)
{
result.W = start.W;
result.X = start.X;
result.Y = start.Y;
result.Z = start.Z;
return;
}
// Calculate temporary values.
Fix64 halfTheta = Fix64.Acos(cosHalfTheta);
Fix64 sinHalfTheta = Fix64.Sqrt(F64.C1 - cosHalfTheta * cosHalfTheta);
Fix64 aFraction = Fix64.Sin((F64.C1 - interpolationAmount) * halfTheta) / sinHalfTheta;
Fix64 bFraction = Fix64.Sin(interpolationAmount * halfTheta) / sinHalfTheta;
//Blend the two quaternions to get the result!
result.X = (Fix64)(start.X * aFraction + end.X * bFraction);
result.Y = (Fix64)(start.Y * aFraction + end.Y * bFraction);
result.Z = (Fix64)(start.Z * aFraction + end.Z * bFraction);
result.W = (Fix64)(start.W * aFraction + end.W * bFraction);
}
public static FixQuaternion RotateTowards(FixQuaternion from, FixQuaternion to, Fix64 maxDegreesDelta)
{
Fix64 dot = Dot(from, to);
if (dot < 0.0f)
{
to = Multiply(to, -1);
dot = -dot;
}
Fix64 halfTheta = Fix64.Acos(dot);
Fix64 theta = halfTheta * 2;
maxDegreesDelta *= Fix64.Deg2Rad;
if (maxDegreesDelta >= theta)
{
return(to);
}
maxDegreesDelta /= theta;
return(Multiply(Multiply(from, Fix64.Sin((1 - maxDegreesDelta) * halfTheta)) + Multiply(to, Fix64.Sin(maxDegreesDelta * halfTheta)), 1 / Fix64.Sin(halfTheta)));
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
//This constraint doesn't consider linear motion.
linearJacobianA = linearJacobianB = new Matrix3x3();
//Compute the world axes.
Vector3 axisA, axisB;
Quaternion.Transform(ref LocalAxisA, ref ConnectionA.Orientation, out axisA);
Quaternion.Transform(ref LocalAxisB, ref ConnectionB.Orientation, out axisB);
Vector3 twistMeasureAxisA, twistMeasureAxisB;
Quaternion.Transform(ref LocalMeasurementAxisA, ref ConnectionA.Orientation, out twistMeasureAxisA);
Quaternion.Transform(ref LocalMeasurementAxisB, ref ConnectionB.Orientation, out twistMeasureAxisB);
//Compute the shortest rotation to bring axisB into alignment with axisA.
Quaternion alignmentRotation;
Quaternion.GetQuaternionBetweenNormalizedVectors(ref axisB, ref axisA, out alignmentRotation);
//Transform the measurement axis on B by the alignment quaternion.
Quaternion.Transform(ref twistMeasureAxisB, ref alignmentRotation, out twistMeasureAxisB);
//We can now compare the angle between the twist axes.
Fix64 error;
Vector3.Dot(ref twistMeasureAxisA, ref twistMeasureAxisB, out error);
error = Fix64.Acos(MathHelper.Clamp(error, -1, F64.C1));
Vector3 cross;
Vector3.Cross(ref twistMeasureAxisA, ref twistMeasureAxisB, out cross);
Fix64 dot;
Vector3.Dot(ref cross, ref axisA, out dot);
if (dot < F64.C0)
{
error = -error;
}
//Compute the bias based upon the error.
velocityBias = new Vector3(errorCorrectionFactor * error, F64.C0, F64.C0);
//We can't just use the axes directly as jacobians. Consider 'cranking' one object around the other.
Vector3 jacobian;
Vector3.Add(ref axisA, ref axisB, out jacobian);
Fix64 lengthSquared = jacobian.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
Vector3.Divide(ref jacobian, Fix64.Sqrt(lengthSquared), out jacobian);
}
else
{
//The constraint is in an invalid configuration. Just ignore it.
jacobian = new Vector3();
}
angularJacobianA = new Matrix3x3 {
M11 = jacobian.X, M12 = jacobian.Y, M13 = jacobian.Z
};
angularJacobianB = new Matrix3x3 {
M11 = -jacobian.X, M12 = -jacobian.Y, M13 = -jacobian.Z
};
}
public static Fix64 AngleFromTo(SVector3 a, SVector3 b)
{
return((Fix64)Fix64.Acos(Dot(a.normalized, b.normalized)));
}
public static Fix64 Angle(FixVector2 a, FixVector2 b)
{
return(Fix64.Acos(a.Normalized * b.Normalized) * Fix64.Rad2Deg);
}
/// <summary>
/// Returns the arc cosine of value.
/// </summary>
public static Fix64 Acos(Fix64 value)
{
return(Fix64.Acos(value));
}
protected internal override void UpdateJacobiansAndVelocityBias()
{
//This constraint doesn't consider linear motion.
linearJacobianA = linearJacobianB = new Matrix3x3();
//Compute the world axes.
Vector3 axisA, axisB;
Quaternion.Transform(ref LocalAxisA, ref ConnectionA.Orientation, out axisA);
Quaternion.Transform(ref LocalAxisB, ref ConnectionB.Orientation, out axisB);
Vector3 twistMeasureAxisA, twistMeasureAxisB;
Quaternion.Transform(ref LocalMeasurementAxisA, ref ConnectionA.Orientation, out twistMeasureAxisA);
Quaternion.Transform(ref LocalMeasurementAxisB, ref ConnectionB.Orientation, out twistMeasureAxisB);
//Compute the shortest rotation to bring axisB into alignment with axisA.
Quaternion alignmentRotation;
Quaternion.GetQuaternionBetweenNormalizedVectors(ref axisB, ref axisA, out alignmentRotation);
//Transform the measurement axis on B by the alignment quaternion.
Quaternion.Transform(ref twistMeasureAxisB, ref alignmentRotation, out twistMeasureAxisB);
//We can now compare the angle between the twist axes.
Fix64 angle;
Vector3.Dot(ref twistMeasureAxisA, ref twistMeasureAxisB, out angle);
angle = Fix64.Acos(MathHelper.Clamp(angle, -1, F64.C1));
//Compute the bias based upon the error.
if (angle > maximumAngle)
{
velocityBias = new Vector3(errorCorrectionFactor * (angle - maximumAngle), F64.C0, F64.C0);
}
else //If the constraint isn't violated, set up the velocity bias to allow a 'speculative' limit.
{
velocityBias = new Vector3(angle - maximumAngle, F64.C0, F64.C0);
}
//We can't just use the axes directly as jacobians. Consider 'cranking' one object around the other.
Vector3 jacobian;
Vector3.Add(ref axisA, ref axisB, out jacobian);
Fix64 lengthSquared = jacobian.LengthSquared();
if (lengthSquared > Toolbox.Epsilon)
{
Vector3.Divide(ref jacobian, Fix64.Sqrt(lengthSquared), out jacobian);
}
else
{
//The constraint is in an invalid configuration. Just ignore it.
jacobian = new Vector3();
}
//In addition to the absolute angle value, we need to know which side of the limit we're hitting.
//The jacobian will be negated on one side. This is because limits can only 'push' in one direction;
//if we didn't flip the direction of the jacobian, it would be trying to push the same direction on both ends of the limit.
//One side would end up doing nothing!
Vector3 cross;
Vector3.Cross(ref twistMeasureAxisA, ref twistMeasureAxisB, out cross);
Fix64 limitSide;
Vector3.Dot(ref cross, ref axisA, out limitSide);
//Negate the jacobian based on what side of the limit we're on.
if (limitSide < F64.C0)
{
Vector3.Negate(ref jacobian, out jacobian);
}
angularJacobianA = new Matrix3x3 {
M11 = jacobian.X, M12 = jacobian.Y, M13 = jacobian.Z
};
angularJacobianB = new Matrix3x3 {
M11 = -jacobian.X, M12 = -jacobian.Y, M13 = -jacobian.Z
};
}