public SCNVector4(SCNVector3 v, pfloat w)
{
X = v.X;
Y = v.Y;
Z = v.Z;
W = w;
}
public SCNVector4(SCNVector4 v)
{
X = v.X;
Y = v.Y;
Z = v.Z;
W = v.W;
}
public SCNVector4(Vector3 v)
{
X = v.X;
Y = v.Y;
Z = v.Z;
W = 0.0f;
}
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.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(pfloat x, pfloat y, pfloat z, pfloat w)
{
X = x;
Y = y;
Z = z;
W = w;
}
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 SCNVector4(Vector2 v)
{
X = v.X;
Y = v.Y;
Z = 0.0f;
W = 0.0f;
}
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;
}
void AssertEqual(SCNMatrix4 matrix, string message,
pfloat m11, pfloat m12, pfloat m13, pfloat m14,
pfloat m21, pfloat m22, pfloat m23, pfloat m24,
pfloat m31, pfloat m32, pfloat m33, pfloat m34,
pfloat m41, pfloat m42, pfloat m43, pfloat m44
)
{
if (CloseEnough(m11, matrix.M11) && CloseEnough(m12, matrix.M12) && CloseEnough(m13, matrix.M13) && CloseEnough(m14, matrix.M14) &&
CloseEnough(m21, matrix.M21) && CloseEnough(m22, matrix.M22) && CloseEnough(m23, matrix.M23) && CloseEnough(m24, matrix.M24) &&
CloseEnough(m31, matrix.M31) && CloseEnough(m32, matrix.M32) && CloseEnough(m33, matrix.M33) && CloseEnough(m34, matrix.M34) &&
CloseEnough(m41, matrix.M41) && CloseEnough(m42, matrix.M42) && CloseEnough(m43, matrix.M43) && CloseEnough(m44, matrix.M44))
{
return;
}
var actualString = matrix.ToString();
var row1 = $"({m11}, {m12}, {m13}, {m14})";
var row2 = $"({m21}, {m22}, {m23}, {m24})";
var row3 = $"({m31}, {m32}, {m33}, {m34})";
var row4 = $"({m41}, {m42}, {m43}, {m44})";
var expectedString = $"{row1}\n{row2}\n{row3}\n{row4}";
Assert.Fail($"Expected matrix:\n{expectedString}\nActual matrix:\n{actualString}\n{message}");
}
public void ToAxisAngle(out SCNVector3 axis, out pfloat angle)
{
SCNVector4 result = ToAxisAngle();
axis = result.Xyz;
angle = result.W;
}
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 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 void NormalizeFast()
{
pfloat scale = (pfloat)MathHelper.InverseSqrtFast(X * X + Y * Y + Z * Z);
X *= scale;
Y *= scale;
Z *= scale;
}
public void Normalize()
{
pfloat scale = 1.0f / this.Length;
X *= scale;
Y *= scale;
Z *= scale;
}
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 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 static SCNVector4 Mult(SCNVector4 a, pfloat f)
{
a.X *= f;
a.Y *= f;
a.Z *= f;
a.W *= f;
return(a);
}
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 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 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 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);
}
void AssertEqual(SCNVector4 vector, string message, pfloat m1, pfloat m2, pfloat m3, pfloat m4)
{
if (m1 == vector.X && m2 == vector.Y && m3 == vector.Z && m4 == vector.W)
{
return;
}
var expectedString = vector.ToString();
var actualString = $"({m1}, {m2}, {m3}, {m4})";
Assert.Fail($"Expected vector:\n{expectedString}\nActual vector:\n{actualString}\n{message}");
}
public SCNQuaternion(pfloat x, pfloat y, pfloat z, pfloat w)
: this(new SCNVector3(x, y, z), w)
{
}
public SCNQuaternion(SCNVector3 v, pfloat w)
{
this.xyz = v;
this.w = w;
}
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);
}
}
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 SCNVector3(pfloat x, pfloat y, pfloat z)
{
X = x;
Y = y;
Z = z;
}
public static void BaryCentric(ref SCNVector3 a, ref SCNVector3 b, ref SCNVector3 c, pfloat u, pfloat v, out SCNVector3 result)
{
result = a; // copy
SCNVector3 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);
}
