/// <summary>
/// Scale a quaternion
/// </summary>
/// <param name="quaternion1">The quaternion to scale.</param>
/// <param name="scaleFactor">Scale factor.</param>
/// <param name="result">The scaled quaternion.</param>
public static void Multiply(ref FPQuaternion quaternion1, FP scaleFactor, out FPQuaternion result)
{
result.x = quaternion1.x * scaleFactor;
result.y = quaternion1.y * scaleFactor;
result.z = quaternion1.z * scaleFactor;
result.w = quaternion1.w * scaleFactor;
}
/// <summary>
/// Quaternions are added.
/// </summary>
/// <param name="quaternion1">The first quaternion.</param>
/// <param name="quaternion2">The second quaternion.</param>
/// <returns>The sum of both quaternions.</returns>
#region public static JQuaternion Add(JQuaternion quaternion1, JQuaternion quaternion2)
public static FPQuaternion Add(FPQuaternion quaternion1, FPQuaternion quaternion2)
{
FPQuaternion result;
FPQuaternion.Add(ref quaternion1, ref quaternion2, out result);
return(result);
}
/// <summary>
/// Quaternions are subtracted.
/// </summary>
/// <param name="quaternion1">The first quaternion.</param>
/// <param name="quaternion2">The second quaternion.</param>
/// <param name="result">The difference of both quaternions.</param>
public static void Subtract(ref FPQuaternion quaternion1, ref FPQuaternion quaternion2, out FPQuaternion result)
{
result.x = quaternion1.x - quaternion2.x;
result.y = quaternion1.y - quaternion2.y;
result.z = quaternion1.z - quaternion2.z;
result.w = quaternion1.w - quaternion2.w;
}
/// <summary>
/// Multiply two quaternions.
/// </summary>
/// <param name="value1">The first quaternion.</param>
/// <param name="value2">The second quaternion.</param>
/// <returns>The product of both quaternions.</returns>
#region public static FP operator *(JQuaternion value1, JQuaternion value2)
public static FPQuaternion operator *(FPQuaternion value1, FPQuaternion value2)
{
FPQuaternion result;
FPQuaternion.Multiply(ref value1, ref value2, out result);
return(result);
}
/// <summary>
/// Creates a quaternion from a matrix.
/// </summary>
/// <param name="matrix">A matrix representing an orientation.</param>
/// <returns>JQuaternion representing an orientation.</returns>
#region public static JQuaternion CreateFromMatrix(JMatrix matrix)
public static FPQuaternion CreateFromMatrix(FPMatrix matrix)
{
FPQuaternion result;
FPQuaternion.CreateFromMatrix(ref matrix, out result);
return(result);
}
/// <summary>
/// Subtract two quaternions.
/// </summary>
/// <param name="value1">The first quaternion.</param>
/// <param name="value2">The second quaternion.</param>
/// <returns>The difference of both quaternions.</returns>
#region public static FP operator -(JQuaternion value1, JQuaternion value2)
public static FPQuaternion operator -(FPQuaternion value1, FPQuaternion value2)
{
FPQuaternion result;
FPQuaternion.Subtract(ref value1, ref value2, out result);
return(result);
}
/// <summary>
/// Add two quaternions.
/// </summary>
/// <param name="value1">The first quaternion.</param>
/// <param name="value2">The second quaternion.</param>
/// <returns>The sum of both quaternions.</returns>
#region public static FP operator +(JQuaternion value1, JQuaternion value2)
public static FPQuaternion operator +(FPQuaternion value1, FPQuaternion value2)
{
FPQuaternion result;
FPQuaternion.Add(ref value1, ref value2, out result);
return(result);
}
/// <summary>
/// Scale a quaternion
/// </summary>
/// <param name="quaternion1">The quaternion to scale.</param>
/// <param name="scaleFactor">Scale factor.</param>
/// <returns>The scaled quaternion.</returns>
#region public static JQuaternion Multiply(JQuaternion quaternion1, FP scaleFactor)
public static FPQuaternion Multiply(FPQuaternion quaternion1, FP scaleFactor)
{
FPQuaternion result;
FPQuaternion.Multiply(ref quaternion1, scaleFactor, out result);
return(result);
}
/// <summary>
/// Multiply two quaternions.
/// </summary>
/// <param name="quaternion1">The first quaternion.</param>
/// <param name="quaternion2">The second quaternion.</param>
/// <returns>The product of both quaternions.</returns>
#region public static JQuaternion Multiply(JQuaternion quaternion1, JQuaternion quaternion2)
public static FPQuaternion Multiply(FPQuaternion quaternion1, FPQuaternion quaternion2)
{
FPQuaternion result;
FPQuaternion.Multiply(ref quaternion1, ref quaternion2, out result);
return(result);
}
/// <summary>
/// Quaternions are added.
/// </summary>
/// <param name="quaternion1">The first quaternion.</param>
/// <param name="quaternion2">The second quaternion.</param>
/// <param name="result">The sum of both quaternions.</param>
public static void Add(ref FPQuaternion quaternion1, ref FPQuaternion quaternion2, out FPQuaternion result)
{
result.x = quaternion1.x + quaternion2.x;
result.y = quaternion1.y + quaternion2.y;
result.z = quaternion1.z + quaternion2.z;
result.w = quaternion1.w + quaternion2.w;
}
/// <summary>
/// Quaternions are subtracted.
/// </summary>
/// <param name="quaternion1">The first quaternion.</param>
/// <param name="quaternion2">The second quaternion.</param>
/// <returns>The difference of both quaternions.</returns>
#region public static JQuaternion Subtract(JQuaternion quaternion1, JQuaternion quaternion2)
public static FPQuaternion Subtract(FPQuaternion quaternion1, FPQuaternion quaternion2)
{
FPQuaternion result;
FPQuaternion.Subtract(ref quaternion1, ref quaternion2, out result);
return(result);
}
public static FPQuaternion LerpUnclamped(FPQuaternion a, FPQuaternion b, FP t)
{
FPQuaternion result = FPQuaternion.Multiply(a, (1 - t)) + FPQuaternion.Multiply(b, t);
result.Normalize();
return(result);
}
public static FPQuaternion FromToRotation(FPVector fromVector, FPVector toVector)
{
FPVector w = FPVector.Cross(fromVector, toVector);
FPQuaternion q = new FPQuaternion(w.x, w.y, w.z, FPVector.Dot(fromVector, toVector));
q.w += FP.Sqrt(fromVector.sqrMagnitude * toVector.sqrMagnitude);
q.Normalize();
return(q);
}
public static FPQuaternion Conjugate(FPQuaternion value)
{
FPQuaternion quaternion;
quaternion.x = -value.x;
quaternion.y = -value.y;
quaternion.z = -value.z;
quaternion.w = value.w;
return(quaternion);
}
private void UpdateChildRotation()
{
FPMatrix matrix = FPMatrix.CreateFromQuaternion(_rotation);
foreach (FPTransform child in tsChildren)
{
child.localRotation = FPQuaternion.CreateFromMatrix(FPMatrix.Inverse(matrix)) * _rotation;
child.localPosition = FPVector.Transform(child.localPosition, FPMatrix.CreateFromQuaternion(child.localRotation));
child.position = TransformPoint(child.localPosition);
}
}
public static FPQuaternion Euler(FP x, FP y, FP z)
{
x *= FP.Deg2Rad;
y *= FP.Deg2Rad;
z *= FP.Deg2Rad;
FPQuaternion rotation;
FPQuaternion.CreateFromYawPitchRoll(y, x, z, out rotation);
return(rotation);
}
public static FP Angle(FPQuaternion a, FPQuaternion b)
{
FPQuaternion aInv = FPQuaternion.Inverse(a);
FPQuaternion f = b * aInv;
FP angle = FP.Acos(f.w) * 2 * FP.Rad2Deg;
if (angle > 180)
{
angle = 360 - angle;
}
return(angle);
}
private void UpdateEditMode()
{
if (transform.hasChanged)
{
_position = transform.position.ToFPVector();
_rotation = transform.rotation.ToFPQuaternion();
_scale = transform.lossyScale.ToFPVector();
_localPosition = transform.localPosition.ToFPVector();
_localRotation = transform.localRotation.ToFPQuaternion();
_localScale = transform.localScale.ToFPVector();
_serialized = true;
}
}
public static FPQuaternion Slerp(FPQuaternion from, FPQuaternion to, FP t)
{
t = FPMath.Clamp(t, 0, 1);
FP dot = Dot(from, to);
if (dot < 0.0f)
{
to = Multiply(to, -1);
dot = -dot;
}
FP halfTheta = FP.Acos(dot);
return(Multiply(Multiply(from, FP.Sin((1 - t) * halfTheta)) + Multiply(to, FP.Sin(t * halfTheta)), 1 / FP.Sin(halfTheta)));
}
/**
* @brief Rotates game object based on provided axis, angle of rotation and relative space.
*
* If relative space is SELF then the game object will rotate based on its forward vector.
**/
public void Rotate(FPVector axis, FP angle, Space relativeTo)
{
FPQuaternion result = FPQuaternion.identity;
if (relativeTo == Space.Self)
{
result = this.rotation * FPQuaternion.AngleAxis(angle, axis);
}
else
{
result = FPQuaternion.AngleAxis(angle, axis) * this.rotation;
}
result.Normalize();
this.rotation = result;
}
/**
* @brief Rotates game object based on provided axis angles and relative space.
*
* If relative space is SELF then the game object will rotate based on its forward vector.
**/
public void Rotate(FPVector eulerAngles, Space relativeTo)
{
FPQuaternion result = FPQuaternion.identity;
if (relativeTo == Space.Self)
{
result = this.rotation * FPQuaternion.Euler(eulerAngles);
}
else
{
result = FPQuaternion.Euler(eulerAngles) * this.rotation;
}
result.Normalize();
this.rotation = result;
}
public static void CreateFromYawPitchRoll(FP yaw, FP pitch, FP roll, out FPQuaternion result)
{
FP num9 = roll * FP.Half;
FP num6 = FP.Sin(num9);
FP num5 = FP.Cos(num9);
FP num8 = pitch * FP.Half;
FP num4 = FP.Sin(num8);
FP num3 = FP.Cos(num8);
FP num7 = yaw * FP.Half;
FP num2 = FP.Sin(num7);
FP num = FP.Cos(num7);
result.x = ((num * num4) * num5) + ((num2 * num3) * num6);
result.y = ((num2 * num3) * num5) - ((num * num4) * num6);
result.z = ((num * num3) * num6) - ((num2 * num4) * num5);
result.w = ((num * num3) * num5) + ((num2 * num4) * num6);
}
/// <summary>
/// Multiply two quaternions.
/// </summary>
/// <param name="quaternion1">The first quaternion.</param>
/// <param name="quaternion2">The second quaternion.</param>
/// <param name="result">The product of both quaternions.</param>
public static void Multiply(ref FPQuaternion quaternion1, ref FPQuaternion quaternion2, out FPQuaternion result)
{
FP x = quaternion1.x;
FP y = quaternion1.y;
FP z = quaternion1.z;
FP w = quaternion1.w;
FP num4 = quaternion2.x;
FP num3 = quaternion2.y;
FP num2 = quaternion2.z;
FP num = quaternion2.w;
FP num12 = (y * num2) - (z * num3);
FP num11 = (z * num4) - (x * num2);
FP num10 = (x * num3) - (y * num4);
FP num9 = ((x * num4) + (y * num3)) + (z * num2);
result.x = ((x * num) + (num4 * w)) + num12;
result.y = ((y * num) + (num3 * w)) + num11;
result.z = ((z * num) + (num2 * w)) + num10;
result.w = (w * num) - num9;
}
/// <summary>
/// Creates a quaternion from a matrix.
/// </summary>
/// <param name="matrix">A matrix representing an orientation.</param>
/// <param name="result">JQuaternion representing an orientation.</param>
public static void CreateFromMatrix(ref FPMatrix matrix, out FPQuaternion result)
{
FP num8 = (matrix.M11 + matrix.M22) + matrix.M33;
if (num8 > FP.Zero)
{
FP num = FP.Sqrt((num8 + FP.One));
result.w = num * FP.Half;
num = FP.Half / num;
result.x = (matrix.M23 - matrix.M32) * num;
result.y = (matrix.M31 - matrix.M13) * num;
result.z = (matrix.M12 - matrix.M21) * num;
}
else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33))
{
FP num7 = FP.Sqrt((((FP.One + matrix.M11) - matrix.M22) - matrix.M33));
FP num4 = FP.Half / num7;
result.x = FP.Half * num7;
result.y = (matrix.M12 + matrix.M21) * num4;
result.z = (matrix.M13 + matrix.M31) * num4;
result.w = (matrix.M23 - matrix.M32) * num4;
}
else if (matrix.M22 > matrix.M33)
{
FP num6 = FP.Sqrt((((FP.One + matrix.M22) - matrix.M11) - matrix.M33));
FP num3 = FP.Half / num6;
result.x = (matrix.M21 + matrix.M12) * num3;
result.y = FP.Half * num6;
result.z = (matrix.M32 + matrix.M23) * num3;
result.w = (matrix.M31 - matrix.M13) * num3;
}
else
{
FP num5 = FP.Sqrt((((FP.One + matrix.M33) - matrix.M11) - matrix.M22));
FP num2 = FP.Half / num5;
result.x = (matrix.M31 + matrix.M13) * num2;
result.y = (matrix.M32 + matrix.M23) * num2;
result.z = FP.Half * num5;
result.w = (matrix.M12 - matrix.M21) * num2;
}
}
private void UpdatePlayMode()
{
if (tsParent != null)
{
_localPosition = tsParent.InverseTransformPoint(position);
FPMatrix matrix = FPMatrix.CreateFromQuaternion(tsParent.rotation);
_localRotation = FPQuaternion.CreateFromMatrix(FPMatrix.Inverse(matrix)) * rotation;
}
else
{
_localPosition = position;
_localRotation = rotation;
}
if (rb != null)
{
if (rb.interpolation == FPRigidBody.InterpolateMode.Interpolate)
{
transform.position = Vector3.Lerp(transform.position, position.ToVector(), Time.deltaTime * DELTA_TIME_FACTOR);
transform.rotation = Quaternion.Lerp(transform.rotation, rotation.ToQuaternion(), Time.deltaTime * DELTA_TIME_FACTOR);
transform.localScale = Vector3.Lerp(transform.localScale, localScale.ToVector(), Time.deltaTime * DELTA_TIME_FACTOR);
return;
}
else if (rb.interpolation == FPRigidBody.InterpolateMode.Extrapolate)
{
transform.position = (position + rb.FPCollider.Body.FPLinearVelocity * Time.deltaTime * DELTA_TIME_FACTOR).ToVector();
transform.rotation = Quaternion.Lerp(transform.rotation, rotation.ToQuaternion(), Time.deltaTime * DELTA_TIME_FACTOR);
transform.localScale = Vector3.Lerp(transform.localScale, localScale.ToVector(), Time.deltaTime * DELTA_TIME_FACTOR);
return;
}
}
transform.position = position.ToVector();
transform.rotation = rotation.ToQuaternion();
transform.localScale = localScale.ToVector();
_scale = transform.lossyScale.ToFPVector();
}
public static FPQuaternion RotateTowards(FPQuaternion from, FPQuaternion to, FP maxDegreesDelta)
{
FP dot = Dot(from, to);
if (dot < 0.0f)
{
to = Multiply(to, -1);
dot = -dot;
}
FP halfTheta = FP.Acos(dot);
FP theta = halfTheta * 2;
maxDegreesDelta *= FP.Deg2Rad;
if (maxDegreesDelta >= theta)
{
return(to);
}
maxDegreesDelta /= theta;
return(Multiply(Multiply(from, FP.Sin((1 - maxDegreesDelta) * halfTheta)) + Multiply(to, FP.Sin(maxDegreesDelta * halfTheta)), 1 / FP.Sin(halfTheta)));
}
static FPQuaternion()
{
identity = new FPQuaternion(0, 0, 0, 1);
}
public void SetFromToRotation(FPVector fromDirection, FPVector toDirection)
{
FPQuaternion targetRotation = FPQuaternion.FromToRotation(fromDirection, toDirection);
this.Set(targetRotation.x, targetRotation.y, targetRotation.z, targetRotation.w);
}
public static FPQuaternion Lerp(FPQuaternion a, FPQuaternion b, FP t)
{
t = FPMath.Clamp(t, FP.Zero, FP.One);
return(LerpUnclamped(a, b, t));
}
/**
* @brief Rotates game object to point forward vector to a target position.
*
* @param target Target position.
**/
public void LookAt(FPVector target)
{
this.rotation = FPQuaternion.CreateFromMatrix(FPMatrix.CreateFromLookAt(position, target));
}
