public fp3x4(uint v)
{
this.c0 = (fp3)v;
this.c1 = (fp3)v;
this.c2 = (fp3)v;
this.c3 = (fp3)v;
}
public void NormalizationTest()
{
var originalVector = new fp3(fp._5, fp.zero, fp.zero);
var modifiedVector = fixmath.Normalize(originalVector);
modifiedVector.Should().Be(new fp3(fp.one, fp.zero, fp.zero));
}
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 static void fp3_static_scalar_constructor()
{
fp3 a = fp3(17.0m);
TestUtils.AreEqual(a.x, 17.0m);
TestUtils.AreEqual(a.y, 17.0m);
TestUtils.AreEqual(a.z, 17.0m);
}
public void MagnitudeTest()
{
var originalVector = new fp3(fp._5, fp._0, fp._0);
var magnitude = fixmath.Magnitude(originalVector);
magnitude.Should().Be(fp._5);
}
public static void fp3_static_constructor()
{
fp3 a = fp3(1, 2, 3);
TestUtils.AreEqual(a.x, 1);
TestUtils.AreEqual(a.y, 2);
TestUtils.AreEqual(a.z, 3);
}
private static fp3 OrthoNormalVectorFast(fp3 n)
{
fp3 res;
if (fpmath.abs(n.z) > VecMath.K1_OVER_SQRT2)
{
// choose p in y-z plane
var a = n.y * n.y + n.z * n.z;
var k = fp.one / fpmath.sqrt(a);
res.x = fp.zero;
res.y = -n.z * k;
res.z = n.y * k;
}
else
{
// choose p in x-y plane
var a = n.x * n.x + n.y * n.y;
var k = fp.one / fpmath.sqrt(a);
res.x = -n.y * k;
res.y = n.x * k;
res.z = fp.zero;
}
return(res);
}
public fp3x4(uint3x4 v)
{
this.c0 = (fp3)v.c0;
this.c1 = (fp3)v.c1;
this.c2 = (fp3)v.c2;
this.c3 = (fp3)v.c3;
}
public static fp3 mul(fp3 a, fp3x3 b)
{
return(fp3(
a.x * b.c0.x + a.y * b.c0.y + a.z * b.c0.z,
a.x * b.c1.x + a.y * b.c1.y + a.z * b.c1.z,
a.x * b.c2.x + a.y * b.c2.y + a.z * b.c2.z));
}
public void MagnitudeSqrTest()
{
var originalVector = new fp3(fp._5, fp.zero, fp.zero);
var magnitude = fixmath.MagnitudeSqr(originalVector);
magnitude.Should().Be(fp._5 * fp._5);
}
public fp3x4(fp v)
{
this.c0 = v;
this.c1 = v;
this.c2 = v;
this.c3 = v;
}
public fp3x4(fp3 c0, fp3 c1, fp3 c2, fp3 c3)
{
this.c0 = c0;
this.c1 = c1;
this.c2 = c2;
this.c3 = c3;
}
public static fp4 mul(fp3 a, fp3x4 b)
{
return(fp4(
a.x * b.c0.x + a.y * b.c0.y + a.z * b.c0.z,
a.x * b.c1.x + a.y * b.c1.y + a.z * b.c1.z,
a.x * b.c2.x + a.y * b.c2.y + a.z * b.c2.z,
a.x * b.c3.x + a.y * b.c3.y + a.z * b.c3.z));
}
public void ReflectTest()
{
var vector = new fp3(fp._5, fp._0, fp._5);
var normal = new fp3(-fp._1, fp.zero, fp.zero);
var reflection = fixmath.Reflect(vector, normal);
reflection.Should().Be(new fp3(-fp._5, fp._0, fp._5));
}
public void ProjectOnPlaneTest()
{
var vector = new fp3(fp._5, fp._1, fp._5);
var normal = new fp3(-fp._1, fp._0, fp._0);
var projection = fixmath.ProjectOnPlane(vector, normal);
projection.Should().Be(new fp3(fp._0, fp._1, fp._5));
}
public void LerpTest()
{
var @from = new fp3(fp._5, fp.zero, fp._5);
var to = new fp3(fp.zero, fp.zero, fp.zero);
var lerped = fixmath.Lerp(@from, to, fp._0_50);
lerped.Should().Be(new fp3(fp._2 + fp._0_50, fp._0, fp._2 + fp._0_50));
}
public void CrossTest()
{
var vector1 = new fp3(fp._1, fp._5, fp._4);
var vector2 = new fp3(fp._2, fp.zero, fp._1);
var cross = fixmath.Cross(vector1, vector2);
cross.Should().Be(new fp3(fp._5, fp._7, -fp._10));
}
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 void MagnitudeClampTest()
{
var originalVector = new fp3(fp._5, fp.zero, fp.zero);
var clampedVector = fixmath.MagnitudeClamp(originalVector, fp._1_10);
clampedVector.x.AsFloat.Should().BeApproximately(1.10f, 0.01f);
clampedVector.y.AsFloat.Should().Be(0f);
clampedVector.z.AsFloat.Should().Be(0f);
}
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 static fp SignedAngle(fp3 lhs, fp3 rhs, fp3 axis)
{
var num1 = VecMath.Angle(lhs, rhs);
var num2 = (lhs.y * rhs.z - lhs.z * rhs.y);
var num3 = (lhs.z * rhs.x - lhs.x * rhs.z);
var num4 = (lhs.x * rhs.y - lhs.y * rhs.x);
var num5 = fpmath.sign(axis.x * num2 + axis.y * num3 + axis.z * num4);
return(num1 * num5);
}
public static fpquaternion FromToRotation(fp3 from, fp3 to)
{
var m = new fpmatrix3x3();
m.SetFromToRotation(from, to);
var q = fpquaternion.zero;
VecMath.MatrixToQuaternion(m, ref q);
return(q);
}
public static void fp3_shuffle_result_1()
{
fp3 a = fp3(0, 1, 2);
fp3 b = fp3(3, 4, 5);
TestUtils.AreEqual(shuffle(a, b, ShuffleComponent.LeftX), (0));
TestUtils.AreEqual(shuffle(a, b, ShuffleComponent.LeftY), (1));
TestUtils.AreEqual(shuffle(a, b, ShuffleComponent.LeftZ), (2));
TestUtils.AreEqual(shuffle(a, b, ShuffleComponent.RightX), (3));
TestUtils.AreEqual(shuffle(a, b, ShuffleComponent.RightY), (4));
TestUtils.AreEqual(shuffle(a, b, ShuffleComponent.RightZ), (5));
}
public void MoveTowardsTest()
{
var current = fp3.one;
var target = new fp3(fp._5, fp.one, fp.one);
var step1 = fixmath.MoveTowards(current, target, fp.one);
step1.Should().Be(new fp3(fp._2, fp.one, fp.one));
var step2 = fixmath.MoveTowards(current, target, fp._10);
step2.Should().Be(new fp3(fp._5, fp.one, fp.one));
}
public void RadiansTest()
{
var vector1 = new fp3(fp._1, fp._5, fp._4);
var vector2 = new fp3(fp._2, fp.zero, fp._1);
var angle = fixmath.Radians(vector1, vector2);
angle.AsInt.Should().Be(1);
vector1 = new fp3(fp._2, fp._1, fp._1);
vector2 = new fp3(fp._2, fp.zero, fp._1);
angle = fixmath.Radians(vector1, vector2);
angle.AsFloat.Should().BeApproximately(0.42f, 0.01f);
}
public static fpquaternion FromToRotationSafe(fp3 lhs, fp3 rhs)
{
var lhsMag = fpmath.length(lhs);
var rhsMag = fpmath.length(rhs);
if (lhsMag < VecMath.VECTOR3_EPSILON || rhsMag < VecMath.VECTOR3_EPSILON)
{
return(fpquaternion.identity);
}
else
{
return(VecMath.FromToRotation(lhs / lhsMag, rhs / rhsMag));
}
}
public void AngleTest()
{
var vector1 = new fp3(fp._1, fp._5, fp._4);
var vector2 = new fp3(fp._2, fp.zero, fp._1);
var angle = fixmath.Angle(vector1, vector2);
angle.AsInt.Should().Be(65);
vector1 = new fp3(fp._2, fp._1, fp._1);
vector2 = new fp3(fp._2, fp.zero, fp._1);
angle = fixmath.Angle(vector1, vector2);
angle.AsInt.Should().Be(24);
}
public void AngleSignedTest()
{
var vector1 = new fp3(fp._1, fp._5, fp._4);
var vector2 = new fp3(fp._2, fp.zero, fp._1);
var angle = fixmath.AngleSigned(vector1, vector2, fp3.up);
angle.AsInt.Should().Be(65);
vector1 = new fp3(-fp._2, fp._1, fp._1);
vector2 = new fp3(fp._2, fp._1, fp._1);
angle = fixmath.AngleSigned(vector1, vector2, fp3.up);
angle.AsFloat.Should().BeApproximately(109.47f, 0.1f);
}
public static void MatrixToQuaternion(fpmatrix3x3 kRot, ref fpquaternion q)
{
// Algorithm in Ken Shoemake's article in 1987 SIGGRAPH course notes
// article "Quaternionf Calculus and Fast Animation".
var fTrace = kRot.Get(0, 0) + kRot.Get(1, 1) + kRot.Get(2, 2);
fp fRoot;
if (fTrace > 0.0f)
{
// |w| > 1/2, may as well choose w > 1/2
fRoot = fpmath.sqrt(fTrace + 1.0f); // 2w
q.w = 0.5f * fRoot;
fRoot = 0.5f / fRoot; // 1/(4w)
q.x = (kRot.Get(2, 1) - kRot.Get(1, 2)) * fRoot;
q.y = (kRot.Get(0, 2) - kRot.Get(2, 0)) * fRoot;
q.z = (kRot.Get(1, 0) - kRot.Get(0, 1)) * fRoot;
}
else
{
// |w| <= 1/2
var sINext = new Vector3Int(1, 2, 0);
var i = 0;
if (kRot.Get(1, 1) > kRot.Get(0, 0))
{
i = 1;
}
if (kRot.Get(2, 2) > kRot.Get(i, i))
{
i = 2;
}
var j = sINext[i];
var k = sINext[j];
fRoot = fpmath.sqrt(kRot.Get(i, i) - kRot.Get(j, j) - kRot.Get(k, k) + 1.0f);
var apkQuat = new fp3(q.x, q.y, q.z);
apkQuat[i] = 0.5f * fRoot;
fRoot = 0.5f / fRoot;
q.w = (kRot.Get(k, j) - kRot.Get(j, k)) * fRoot;
apkQuat[j] = (kRot.Get(j, i) + kRot.Get(i, j)) * fRoot;
apkQuat[k] = (kRot.Get(k, i) + kRot.Get(i, k)) * fRoot;
q.x = apkQuat[0];
q.y = apkQuat[1];
q.z = apkQuat[2];
}
q = q.normalized;
}
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));
}
}
