public SCNQuaternion(ref Matrix3 matrix) { double scale = System.Math.Pow(matrix.Determinant, 1.0d / 3.0d); float x, y, z; w = (float)(System.Math.Sqrt(System.Math.Max(0, scale + matrix[0, 0] + matrix[1, 1] + matrix[2, 2])) / 2); x = (float)(System.Math.Sqrt(System.Math.Max(0, scale + matrix[0, 0] - matrix[1, 1] - matrix[2, 2])) / 2); y = (float)(System.Math.Sqrt(System.Math.Max(0, scale - matrix[0, 0] + matrix[1, 1] - matrix[2, 2])) / 2); z = (float)(System.Math.Sqrt(System.Math.Max(0, scale - matrix[0, 0] - matrix[1, 1] + matrix[2, 2])) / 2); xyz = new Vector3(x, y, z); if (matrix[2, 1] - matrix[1, 2] < 0) { X = -X; } if (matrix[0, 2] - matrix[2, 0] < 0) { Y = -Y; } if (matrix[1, 0] - matrix[0, 1] < 0) { Z = -Z; } }
/// <summary> /// Convert the current quaternion to axis angle representation /// </summary> /// <param name="axis">The resultant axis</param> /// <param name="angle">The resultant angle</param> public void ToAxisAngle(out SCNVector3 axis, out pfloat angle) { SCNVector4 result = ToAxisAngle(); axis = result.Xyz; angle = result.W; }
/// <summary> /// Build a quaternion from the given axis and angle /// </summary> /// <param name="axis">The axis to rotate about</param> /// <param name="angle">The rotation angle in radians</param> /// <returns></returns> public static SCNQuaternion FromAxisAngle(SCNVector3 axis, float angle) { if (axis.LengthSquared == 0.0f) { return(Identity); } SCNQuaternion result = Identity; angle *= 0.5f; axis.Normalize(); result.Xyz = axis * (float)System.Math.Sin(angle); result.W = (float)System.Math.Cos(angle); return(Normalize(result)); }
/// <summary> /// Construct a new SCNQuaternion from vector and w components /// </summary> /// <param name="v">The vector part</param> /// <param name="w">The w part</param> public SCNQuaternion(SCNVector3 v, pfloat w) { this.xyz = v; this.w = w; }
/// <summary> /// Do Spherical linear interpolation between two quaternions /// </summary> /// <param name="q1">The first quaternion</param> /// <param name="q2">The second quaternion</param> /// <param name="blend">The blend factor</param> /// <returns>A smooth blend between the given quaternions</returns> public static SCNQuaternion Slerp(SCNQuaternion q1, SCNQuaternion q2, float blend) { // if either input is zero, return the other. if (q1.LengthSquared == 0.0f) { if (q2.LengthSquared == 0.0f) { return(Identity); } return(q2); } else if (q2.LengthSquared == 0.0f) { return(q1); } pfloat cosHalfAngle = q1.W * q2.W + SCNVector3.Dot(q1.Xyz, q2.Xyz); if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f) { // angle = 0.0f, so just return one input. return(q1); } else if (cosHalfAngle < 0.0f) { q2.Xyz = -q2.Xyz; q2.W = -q2.W; cosHalfAngle = -cosHalfAngle; } float blendA; float blendB; if (cosHalfAngle < 0.99f) { // do proper slerp for big angles float halfAngle = (float)System.Math.Acos(cosHalfAngle); float sinHalfAngle = (float)System.Math.Sin(halfAngle); float oneOverSinHalfAngle = 1.0f / sinHalfAngle; blendA = (float)System.Math.Sin(halfAngle * (1.0f - blend)) * oneOverSinHalfAngle; blendB = (float)System.Math.Sin(halfAngle * blend) * oneOverSinHalfAngle; } else { // do lerp if angle is really small. blendA = 1.0f - blend; blendB = blend; } SCNQuaternion result = new SCNQuaternion(blendA * q1.Xyz + blendB * q2.Xyz, blendA * q1.W + blendB * q2.W); if (result.LengthSquared > 0.0f) { return(Normalize(result)); } else { return(Identity); } }
/// <summary> /// Multiplies two instances. /// </summary> /// <param name="left">The first instance.</param> /// <param name="right">The second instance.</param> /// <param name="result">A new instance containing the result of the calculation.</param> public static void Multiply(ref SCNQuaternion left, ref SCNQuaternion right, out SCNQuaternion result) { result = new SCNQuaternion( right.W * left.Xyz + left.W * right.Xyz + SCNVector3.Cross(left.Xyz, right.Xyz), left.W * right.W - SCNVector3.Dot(left.Xyz, right.Xyz)); }
public static SCNQuaternion Mult(SCNQuaternion left, SCNQuaternion right) { return(new SCNQuaternion( right.W * left.Xyz + left.W * right.Xyz + SCNVector3.Cross(left.Xyz, right.Xyz), left.W * right.W - SCNVector3.Dot(left.Xyz, right.Xyz))); }