public SCNQuaternion(ref Matrix3 matrix)
{
#if NET
double scale = System.Math.Pow(matrix.GetDeterminant(), 1.0d / 3.0d);
#else
double scale = System.Math.Pow(matrix.Determinant, 1.0d / 3.0d);
#endif
float x, y, z;
w = (float)(System.Math.Sqrt(System.Math.Max(0, scale + matrix.R0C0 + matrix.R1C1 + matrix.R2C2)) / 2);
x = (float)(System.Math.Sqrt(System.Math.Max(0, scale + matrix.R0C0 - matrix.R1C1 - matrix.R2C2)) / 2);
y = (float)(System.Math.Sqrt(System.Math.Max(0, scale - matrix.R0C0 + matrix.R1C1 - matrix.R2C2)) / 2);
z = (float)(System.Math.Sqrt(System.Math.Max(0, scale - matrix.R0C0 - matrix.R1C1 + matrix.R2C2)) / 2);
xyz = new Vector3(x, y, z);
if (matrix.R2C1 - matrix.R1C2 < 0)
{
X = -X;
}
if (matrix.R0C2 - matrix.R2C0 < 0)
{
Y = -Y;
}
if (matrix.R1C0 - matrix.R0C1 < 0)
{
Z = -Z;
}
}
public SCNVector4(SCNVector3 v, pfloat w)
{
X = v.X;
Y = v.Y;
Z = v.Z;
W = w;
}
public void ToAxisAngle(out SCNVector3 axis, out pfloat angle)
{
SCNVector4 result = ToAxisAngle();
axis = result.Xyz;
angle = result.W;
}
public static void Lerp(ref SCNVector4 a, ref SCNVector4 b, pfloat blend, out SCNVector4 result)
{
result.X = blend * (b.X - a.X) + a.X;
result.Y = blend * (b.Y - a.Y) + a.Y;
result.Z = blend * (b.Z - a.Z) + a.Z;
result.W = blend * (b.W - a.W) + a.W;
}
public SCNVector4(SCNVector4 v)
{
X = v.X;
Y = v.Y;
Z = v.Z;
W = v.W;
}
public static void Mult(ref SCNVector4 a, pfloat f, out SCNVector4 result)
{
result.X = a.X * f;
result.Y = a.Y * f;
result.Z = a.Z * f;
result.W = a.W * f;
}
public SCNVector4(Vector3 v)
{
X = v.X;
Y = v.Y;
Z = v.Z;
W = 0.0f;
}
public SCNVector4 ToAxisAngle()
{
SCNQuaternion q = this;
if (q.W > 1.0f)
{
q.Normalize();
}
SCNVector4 result = new SCNVector4();
result.W = 2.0f * (pfloat)System.Math.Acos(q.W); // angle
pfloat den = (pfloat)System.Math.Sqrt(1.0 - q.W * q.W);
if (den > 0.0001f)
{
result.Xyz = q.Xyz / den;
}
else
{
// This occurs when the angle is zero.
// Not a problem: just set an arbitrary normalized axis.
result.Xyz = SCNVector3.UnitX;
}
return(result);
}
public SCNVector4(pfloat x, pfloat y, pfloat z, pfloat w)
{
X = x;
Y = y;
Z = z;
W = w;
}
public SCNVector4(Vector2 v)
{
X = v.X;
Y = v.Y;
Z = 0.0f;
W = 0.0f;
}
public static SCNVector3 Lerp(SCNVector3 a, SCNVector3 b, pfloat blend)
{
a.X = blend * (b.X - a.X) + a.X;
a.Y = blend * (b.Y - a.Y) + a.Y;
a.Z = blend * (b.Z - a.Z) + a.Z;
return(a);
}
public static SCNVector4 Lerp(SCNVector4 a, SCNVector4 b, pfloat blend)
{
a.X = blend * (b.X - a.X) + a.X;
a.Y = blend * (b.Y - a.Y) + a.Y;
a.Z = blend * (b.Z - a.Z) + a.Z;
a.W = blend * (b.W - a.W) + a.W;
return(a);
}
public static SCNVector4 Mult(SCNVector4 a, pfloat f)
{
a.X *= f;
a.Y *= f;
a.Z *= f;
a.W *= f;
return(a);
}
public void Normalize()
{
pfloat scale = 1.0f / this.Length;
X *= scale;
Y *= scale;
Z *= scale;
}
public static void NormalizeFast(ref SCNVector3 vec, out SCNVector3 result)
{
pfloat scale = (pfloat)MathHelper.InverseSqrtFast(vec.X * vec.X + vec.Y * vec.Y + vec.Z * vec.Z);
result.X = vec.X * scale;
result.Y = vec.Y * scale;
result.Z = vec.Z * scale;
}
public static void Normalize(ref SCNVector3 vec, out SCNVector3 result)
{
pfloat scale = 1.0f / vec.Length;
result.X = vec.X * scale;
result.Y = vec.Y * scale;
result.Z = vec.Z * scale;
}
public void NormalizeFast()
{
pfloat scale = (pfloat)MathHelper.InverseSqrtFast(X * X + Y * Y + Z * Z);
X *= scale;
Y *= scale;
Z *= scale;
}
public static void Div(ref SCNVector4 a, pfloat f, out SCNVector4 result)
{
pfloat mult = 1.0f / f;
result.X = a.X * mult;
result.Y = a.Y * mult;
result.Z = a.Z * mult;
result.W = a.W * mult;
}
public static SCNVector3 NormalizeFast(SCNVector3 vec)
{
pfloat scale = (pfloat)MathHelper.InverseSqrtFast(vec.X * vec.X + vec.Y * vec.Y + vec.Z * vec.Z);
vec.X *= scale;
vec.Y *= scale;
vec.Z *= scale;
return(vec);
}
public static SCNVector3 Normalize(SCNVector3 vec)
{
pfloat scale = 1.0f / vec.Length;
vec.X *= scale;
vec.Y *= scale;
vec.Z *= scale;
return(vec);
}
public static SCNVector3 operator /(SCNVector3 vec, pfloat scale)
{
pfloat mult = 1.0f / scale;
vec.X *= mult;
vec.Y *= mult;
vec.Z *= mult;
return(vec);
}
public static SCNVector4 Div(SCNVector4 a, pfloat f)
{
pfloat mult = 1.0f / f;
a.X *= mult;
a.Y *= mult;
a.Z *= mult;
a.W *= mult;
return(a);
}
public static void BaryCentric(ref SCNVector4 a, ref SCNVector4 b, ref SCNVector4 c, pfloat u, pfloat v, out SCNVector4 result)
{
result = a; // copy
SCNVector4 temp = b; // copy
Subtract(ref temp, ref a, out temp);
Multiply(ref temp, u, out temp);
Add(ref result, ref temp, out result);
temp = c; // copy
Subtract(ref temp, ref a, out temp);
Multiply(ref temp, v, out temp);
Add(ref result, ref temp, out result);
}
public static SCNVector4 BaryCentric(SCNVector4 a, SCNVector4 b, SCNVector4 c, pfloat u, pfloat v)
{
return(a + u * (b - a) + v * (c - a));
}
public static void CalculateAngle(ref SCNVector3 first, ref SCNVector3 second, out pfloat result)
{
pfloat temp;
SCNVector3.Dot(ref first, ref second, out temp);
result = (pfloat)System.Math.Acos(temp / (first.Length * second.Length));
}
public static void Dot(ref SCNVector4 left, ref SCNVector4 right, out pfloat result)
{
result = left.X * right.X + left.Y * right.Y + left.Z * right.Z + left.W * right.W;
}
public static void Divide(ref SCNVector4 vector, pfloat scale, out SCNVector4 result)
{
Multiply(ref vector, 1 / scale, out result);
}
public static SCNVector4 Divide(SCNVector4 vector, pfloat scale)
{
Divide(ref vector, scale, out vector);
return(vector);
}
public static void Multiply(ref SCNVector4 vector, pfloat scale, out SCNVector4 result)
{
result = new SCNVector4(vector.X * scale, vector.Y * scale, vector.Z * scale, vector.W * scale);
}
public static SCNVector4 Multiply(SCNVector4 vector, pfloat scale)
{
Multiply(ref vector, scale, out vector);
return(vector);
}
