/// <summary>
/// Clamps a quaternion rotation so that it remains within the specified angle
/// from identity.
/// </summary>
public static bool Clamp(ref Quaternion q, float cosineOfClampAngle)
{
float cosineOfHalfClampAngle = CosineOfHalfAngleFromCosine(cosineOfClampAngle);
if (q.w >= cosineOfHalfClampAngle)
{
return(false); // already inside of the clamp
}
if (q.w > 0.99999f)
{
q = Quaternion.identity;
return(true);
}
float s = SineFromCosine(q.w);
OVRDebugUtils.Assert(s > OVRMathUtils.FltSmallestNonDenormal);
Vector3 axis;
axis.x = q.x / s;
axis.y = q.y / s;
axis.z = q.z / s;
axis.Normalize();
float sineOfHalfClampAngle = SineFromCosine(cosineOfHalfClampAngle);
q.x = axis.x * sineOfHalfClampAngle;
q.y = axis.y * sineOfHalfClampAngle;
q.z = axis.z * sineOfHalfClampAngle;
q.w = cosineOfHalfClampAngle;
return(true);
}
/// <summary>
/// Constructs a quaternion that will rotate the from vector to the to vector.
/// This assumes 'from' and 'to' are normalized.
/// </summary>
public static Quaternion RotationBetweenTwoVectors(Vector3 from, Vector3 to)
{
/*
* float dot = Vector3.Dot( from, to );
* if ( dot > 0.9999f )
* {
* OVRDebugUtils.Assert( dot <= 0.9999f );
* return Quaternion.identity;
* }
*
* Quaternion q;
* q.w = CosineOfHalfAngleFromCosine( dot );
* float sineHalfAngle = SineFromCosine( q.w );
*
* // get a vector orthogonal to both from and to
* Vector3 axis = Vector3.Cross( from, to );
* OVRDebugUtils.Assert( axis.magnitude > 0.0001f );
* q.x = axis.x * sineHalfAngle;
* q.y = axis.y * sineHalfAngle;
* q.z = axis.z * sineHalfAngle;
*
* return q;
*/
Quaternion q;
q.x = from.y * to.z - from.z * to.y;
q.y = from.z * to.x - from.x * to.z;
q.z = from.x * to.y - from.y * to.x;
q.w = to.x * from.x + to.y * from.y + to.z * from.z + 1.0f;
OVRDebugUtils.Assert(q.w > 1e-18f);
Normalize(ref q);
return(q);
}
/// <summary>
/// Normalizes a quaternion. This can be necessary to repair floating point error.
/// </summary>
public static void Normalize(ref Quaternion q)
{
float mag = Mathf.Sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w);
// if the magnitude is becoming very small, let us know that we're risking a denormal
OVRDebugUtils.Assert(mag > FltSmallestNonDenormal);
float inverseMag = 1.0f / mag;
q.x *= inverseMag;
q.y *= inverseMag;
q.z *= inverseMag;
q.w *= inverseMag;
}
/// <summary>
/// Adds a mapping from a joystick to a behavior.
/// </summary>
public static void AddInputMapping(int joystickNumber, MonoBehaviour comp)
{
for (int i = 0; i < inputMap.Count; ++i)
{
InputMapping im = inputMap[i];
if (im.component == comp && im.joystickNumber == joystickNumber)
{
OVRDebugUtils.Assert(false, "Input mapping already exists!");
return;
}
}
inputMap.Add(new InputMapping(comp, joystickNumber));
}
/// <summary>
/// Returns the dot product of two quaternions. Note this is not the cosine
/// of the angle between the quaternions' forward vectors, but the cosine of
/// half the angle between them.
/// </summary>
public static float QuaternionDot(Quaternion a, Quaternion b)
{
OVRDebugUtils.Assert(QuaternionIsNormalized(a, 0.001f));
OVRDebugUtils.Assert(QuaternionIsNormalized(b, 0.001f));
return(a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w);
}