public fp2x3(fp m00, fp m01, fp m02,
fp m10, fp m11, fp m12)
{
this.c0 = new fp2(m00, m10);
this.c1 = new fp2(m01, m11);
this.c2 = new fp2(m02, m12);
}
public fpquaternion(fp x, fp y, fp z, fp w)
{
this.x = x;
this.y = y;
this.z = z;
this.w = w;
}
public fp2x4(fp v)
{
this.c0 = v;
this.c1 = v;
this.c2 = v;
this.c3 = v;
}
public fpquaternion(fp4 value)
{
this.x = value.x;
this.y = value.y;
this.z = value.z;
this.w = value.w;
}
public fp3x2(fp m00, fp m01,
fp m10, fp m11,
fp m20, fp m21)
{
this.c0 = new fp3(m00, m10, m20);
this.c1 = new fp3(m01, m11, m21);
}
public void Normalize()
{
var v = fpmath.normalizesafe(this);
this.x = v.x;
this.y = v.y;
}
public fpquaternion(fp3x3 m)
{
var u = m.c0;
var v = m.c1;
var w = m.c2;
var u_sign = (uint)u.x & 0x80000000;
var t = v.y + (fp)((uint)w.z ^ u_sign);
var u_mask = new uint4((int)u_sign >> 31);
var t_mask = new uint4((int)t >> 31);
var tr = 1.0f + fpmath.abs(u.x);
var sign_flips = new uint4(0x00000000, 0x80000000, 0x80000000, 0x80000000) ^ (u_mask & new uint4(0x00000000, 0x80000000, 0x00000000, 0x80000000)) ^
(t_mask & new uint4(0x80000000, 0x80000000, 0x80000000, 0x00000000));
var value = new float4(tr, u.y, w.x, v.z) + (float4)((uint4) new float4(t, v.x, u.z, w.y) ^ sign_flips); // +---, +++-, ++-+, +-++
value = ((uint4)value & ~u_mask) | ((uint4)value.zwxy & u_mask);
value = ((uint4)value.wzyx & ~t_mask) | ((uint4)value & t_mask);
value = fpmath.normalize(value);
this.x = value.x;
this.y = value.y;
this.z = value.z;
this.w = value.w;
}
public fp4x2(fp m00, fp m01,
fp m10, fp m11,
fp m20, fp m21,
fp m30, fp m31)
{
this.c0 = new fp4(m00, m10, m20, m30);
this.c1 = new fp4(m01, m11, m21, m31);
}
public fp2x4(fp m00, fp m01, fp m02, fp m03,
fp m10, fp m11, fp m12, fp m13)
{
this.c0 = new fp2(m00, m10);
this.c1 = new fp2(m01, m11);
this.c2 = new fp2(m02, m12);
this.c3 = new fp2(m03, m13);
}
public static fp3x4 fp3x4(fp m00, fp m01, fp m02, fp m03,
fp m10, fp m11, fp m12, fp m13,
fp m20, fp m21, fp m22, fp m23)
{
return(new fp3x4(m00, m01, m02, m03,
m10, m11, m12, m13,
m20, m21, m22, m23));
}
public static fp3x2 fp3x2(fp m00, fp m01,
fp m10, fp m11,
fp m20, fp m21)
{
return(new fp3x2(m00, m01,
m10, m11,
m20, m21));
}
public static fp3x3 fp3x3(fp m00, fp m01, fp m02,
fp m10, fp m11, fp m12,
fp m20, fp m21, fp m22)
{
return(new fp3x3(m00, m01, m02,
m10, m11, m12,
m20, m21, m22));
}
public fp3x3(fp m00, fp m01, fp m02,
fp m10, fp m11, fp m12,
fp m20, fp m21, fp m22)
{
this.c0 = new fp3(m00, m10, m20);
this.c1 = new fp3(m01, m11, m21);
this.c2 = new fp3(m02, m12, m22);
}
public fp4x3(fp m00, fp m01, fp m02,
fp m10, fp m11, fp m12,
fp m20, fp m21, fp m22,
fp m30, fp m31, fp m32)
{
this.c0 = new fp4(m00, m10, m20, m30);
this.c1 = new fp4(m01, m11, m21, m31);
this.c2 = new fp4(m02, m12, m22, m32);
}
public fp3x4(fp m00, fp m01, fp m02, fp m03,
fp m10, fp m11, fp m12, fp m13,
fp m20, fp m21, fp m22, fp m23)
{
this.c0 = new fp3(m00, m10, m20);
this.c1 = new fp3(m01, m11, m21);
this.c2 = new fp3(m02, m12, m22);
this.c3 = new fp3(m03, m13, m23);
}
public fp4x4(fp m00, fp m01, fp m02, fp m03,
fp m10, fp m11, fp m12, fp m13,
fp m20, fp m21, fp m22, fp m23,
fp m30, fp m31, fp m32, fp m33)
{
this.c0 = new fp4(m00, m10, m20, m30);
this.c1 = new fp4(m01, m11, m21, m31);
this.c2 = new fp4(m02, m12, m22, m32);
this.c3 = new fp4(m03, m13, m23, m33);
}
public static fp ClampedMove(fp lhs, fp rhs, fp clampedDelta)
{
var delta = rhs - lhs;
if (delta > fp.zero)
{
return(lhs + fpmath.min(delta, clampedDelta));
}
else
{
return(lhs - fpmath.min(-delta, clampedDelta));
}
}
public void Inverse()
{
if (this == fpquaternion.zero)
{
return;
}
var num2 = this.x * this.x + this.y * this.y + this.z * this.z + this.w * this.w;
var num = 1f / num2;
this.x = -this.x * num;
this.y = -this.y * num;
this.z = -this.z * num;
this.w = this.w * num;
}
public static fp3 RotateTowards(fp3 lhs, fp3 rhs, fp angleMove, fp magnitudeMove)
{
var lhsMag = fpmath.length(lhs);
var rhsMag = fpmath.length(rhs);
// both vectors are non-zero
if (lhsMag > VecMath.VECTOR3_EPSILON && rhsMag > VecMath.VECTOR3_EPSILON)
{
fp3 lhsNorm = lhs / lhsMag;
fp3 rhsNorm = rhs / rhsMag;
var dot = fpmath.dot(lhsNorm, rhsNorm);
// direction is almost the same
if (dot > fp.one - VecMath.VECTOR3_EPSILON)
{
return(VecMath.MoveTowards(lhs, rhs, magnitudeMove));
}
// directions are almost opposite
else if (dot < -fp.one + VecMath.VECTOR3_EPSILON)
{
var axis = VecMath.OrthoNormalVectorFast(lhsNorm);
var m = fpmatrix3x3.zero;
m.SetAxisAngle(axis, angleMove);
var rotated = m.MultiplyVector3(lhsNorm);
rotated *= VecMath.ClampedMove(lhsMag, rhsMag, magnitudeMove);
return(rotated);
}
// normal case
else
{
var angle = fpmath.acos(dot);
var axis = fpmath.normalize(fpmath.cross(lhsNorm, rhsNorm));
var m = fpmatrix3x3.zero;
m.SetAxisAngle(axis, fpmath.min(angleMove, angle));
var rotated = m.MultiplyVector3(lhsNorm);
rotated *= VecMath.ClampedMove(lhsMag, rhsMag, magnitudeMove);
return(rotated);
}
}
// at least one of the vectors is almost zero
else
{
return(VecMath.MoveTowards(lhs, rhs, magnitudeMove));
}
}
public static fp3 Slerp(fp3 lhs, fp3 rhs, fp t)
{
var lhsMag = fpmath.length(lhs);
var rhsMag = fpmath.length(rhs);
if (lhsMag < VecMath.VECTOR3_EPSILON || rhsMag < VecMath.VECTOR3_EPSILON)
{
return(VecMath.Lerp(lhs, rhs, t));
}
var lerpedMagnitude = fpmath.lerp(lhsMag, rhsMag, t);
var dot = fpmath.dot(lhs, rhs) / (lhsMag * rhsMag);
// direction is almost the same
if (dot > 1.0F - VecMath.VECTOR3_EPSILON)
{
return(VecMath.Lerp(lhs, rhs, t));
}
// directions are almost opposite
else if (dot < -1.0F + VecMath.VECTOR3_EPSILON)
{
var lhsNorm = lhs / lhsMag;
var axis = VecMath.OrthoNormalVectorFast(lhsNorm);
var m = fpmatrix3x3.zero;
m.SetAxisAngle(axis, fpmath.PI * t);
var slerped = m.MultiplyVector3(lhsNorm);
slerped *= lerpedMagnitude;
return(slerped);
}
// normal case
else
{
var axis = fpmath.cross(lhs, rhs);
var lhsNorm = lhs / lhsMag;
axis = fpmath.normalizesafe(axis);
var angle = fpmath.acos(dot) * t;
var m = fpmatrix3x3.zero;
m.SetAxisAngle(axis, angle);
var slerped = m.MultiplyVector3(lhsNorm);
slerped *= lerpedMagnitude;
return(slerped);
}
}
public static fpquaternion Euler(fp roll, fp pitch, fp yaw)
{
roll *= fpmath.PI / 180f;
pitch *= fpmath.PI / 180f;
yaw *= fpmath.PI / 180f;
fpquaternion quaternion;
var num9 = roll * 0.5f;
var num6 = fpmath.sin(num9);
var num5 = fpmath.cos(num9);
var num8 = pitch * 0.5f;
var num4 = fpmath.sin(num8);
var num3 = fpmath.cos(num8);
var num7 = yaw * 0.5f;
var num2 = fpmath.sin(num7);
var num = fpmath.cos(num7);
quaternion.x = num * num4 * num5 + num2 * num3 * num6;
quaternion.y = num2 * num3 * num5 - num * num4 * num6;
quaternion.z = num * num3 * num6 - num2 * num4 * num5;
quaternion.w = num * num3 * num5 + num2 * num4 * num6;
return(quaternion);
}
public static fp3 MoveTowards(fp3 lhs, fp3 rhs, fp clampedDistance)
{
var delta = rhs - lhs;
var sqrDelta = fpmath.lengthsq(delta);
var sqrClampedDistance = clampedDistance * clampedDistance;
if (sqrDelta > sqrClampedDistance)
{
var deltaMag = fpmath.sqrt(sqrDelta);
if (deltaMag > VecMath.VECTOR3_EPSILON)
{
return(lhs + delta / deltaMag * clampedDistance);
}
else
{
return(lhs);
}
}
else
{
return(rhs);
}
}
public static fpquaternion Slerp(fpquaternion quaternion1, fpquaternion quaternion2, fp amount)
{
fp num2;
fp num3;
fpquaternion quaternion;
var num = amount;
var num4 = quaternion1.x * quaternion2.x + quaternion1.y * quaternion2.y + quaternion1.z * quaternion2.z + quaternion1.w * quaternion2.w;
var flag = false;
if (num4 < 0f)
{
flag = true;
num4 = -num4;
}
if (num4 > 0.999999f)
{
num3 = 1f - num;
num2 = flag ? -num : num;
}
else
{
var num5 = fpmath.acos(num4);
var num6 = 1.0f / fpmath.sin(num5);
num3 = fpmath.sin((1f - num) * num5) * num6;
num2 = flag ? -fpmath.sin(num * num5) * num6 : fpmath.sin(num * num5) * num6;
}
quaternion.x = num3 * quaternion1.x + num2 * quaternion2.x;
quaternion.y = num3 * quaternion1.y + num2 * quaternion2.y;
quaternion.z = num3 * quaternion1.z + num2 * quaternion2.z;
quaternion.w = num3 * quaternion1.w + num2 * quaternion2.w;
return(quaternion);
}
public fp4x2(fp v)
{
this.c0 = v;
this.c1 = v;
}
public static fp4x2 fp4x2(fp v)
{
return(new fp4x2(v));
}
public static fp2x3 fp2x3(fp v)
{
return(new fp2x3(v));
}
public static fp2x3 fp2x3(fp m00, fp m01, fp m02,
fp m10, fp m11, fp m12)
{
return(new fp2x3(m00, m01, m02,
m10, m11, m12));
}
public fp2x2(fp m00, fp m01,
fp m10, fp m11)
{
this.c0 = new fp2(m00, m10);
this.c1 = new fp2(m01, m11);
}
public fp2x3(fp v)
{
this.c0 = v;
this.c1 = v;
this.c2 = v;
}
public static fp2x2 fp2x2(fp m00, fp m01,
fp m10, fp m11)
{
return(new fp2x2(m00, m01,
m10, m11));
}