public SCNVector4(SCNVector3 v, pfloat w)
{
X = v.X;
Y = v.Y;
Z = v.Z;
W = w;
}
public SCNVector4(SCNVector3 v)
{
X = v.X;
Y = v.Y;
Z = v.Z;
W = 0.0f;
}
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 void ToAxisAngle(out SCNVector3 axis, out pfloat angle)
{
SCNVector4 result = ToAxisAngle();
axis = result.Xyz;
angle = result.W;
}
/// <summary>
/// Build a rotation matrix from the specified axis/angle rotation.
/// </summary>
/// <param name="axis">The axis to rotate about.</param>
/// <param name="angle">Angle in radians to rotate counter-clockwise (looking in the direction of the given axis).</param>
/// <param name="result">A matrix instance.</param>
public static void CreateFromAxisAngle(SCNVector3 axis, pfloat angle, out SCNMatrix4 result)
{
pfloat cos = (float)System.Math.Cos(-angle);
pfloat sin = (float)System.Math.Sin(-angle);
pfloat t = 1.0f - cos;
axis.Normalize();
result = new SCNMatrix4(t * axis.X * axis.X + cos, t * axis.X * axis.Y - sin * axis.Z, t * axis.X * axis.Z + sin * axis.Y, 0.0f,
t * axis.X * axis.Y + sin * axis.Z, t * axis.Y * axis.Y + cos, t * axis.Y * axis.Z - sin * axis.X, 0.0f,
t * axis.X * axis.Z - sin * axis.Y, t * axis.Y * axis.Z + sin * axis.X, t * axis.Z * axis.Z + cos, 0.0f,
0, 0, 0, 1);
}
/// <summary>
/// Build a world space to camera space matrix
/// </summary>
/// <param name="eye">Eye (camera) position in world space</param>
/// <param name="target">Target position in world space</param>
/// <param name="up">Up vector in world space (should not be parallel to the camera direction, that is target - eye)</param>
/// <returns>A SCNMatrix4 that transforms world space to camera space</returns>
public static SCNMatrix4 LookAt(SCNVector3 eye, SCNVector3 target, SCNVector3 up)
{
SCNVector3 z = SCNVector3.Normalize(eye - target);
SCNVector3 x = SCNVector3.Normalize(SCNVector3.Cross(up, z));
SCNVector3 y = SCNVector3.Normalize(SCNVector3.Cross(z, x));
SCNMatrix4 rot = new SCNMatrix4(new SCNVector4(x.X, y.X, z.X, 0.0f),
new SCNVector4(x.Y, y.Y, z.Y, 0.0f),
new SCNVector4(x.Z, y.Z, z.Z, 0.0f),
SCNVector4.UnitW);
SCNMatrix4 trans = SCNMatrix4.CreateTranslation(-eye);
return(trans * rot);
}
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>
/// Build a scaling matrix
/// </summary>
/// <param name="scale">Scale factors for x,y and z axes</param>
/// <returns>A scaling matrix</returns>
public static SCNMatrix4 Scale(SCNVector3 scale)
{
return(Scale(scale.X, scale.Y, scale.Z));
}
/// <summary>
/// Creates a translation matrix.
/// </summary>
/// <param name="vector">The translation vector.</param>
/// <param name="result">The resulting SCNMatrix4 instance.</param>
public static void CreateTranslation(ref SCNVector3 vector, out SCNMatrix4 result)
{
result = Identity;
result.Row3 = new SCNVector4(vector.X, vector.Y, vector.Z, 1);
}
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 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)));
}
