public static Vector4 Normalize(Vector4 vector)
{
if (Vector.IsHardwareAccelerated)
{
Single length = vector.Length();
return(vector / length);
}
else
{
Single ls = vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z + vector.W * vector.W;
Single invNorm = 1.0f / MathF.Sqrt(ls);
return(new Vector4(
vector.X * invNorm,
vector.Y * invNorm,
vector.Z * invNorm,
vector.W * invNorm));
}
}
/// <summary>
/// Interpolates between two quaternions, using spherical linear interpolation.
/// </summary>
/// <param name="quaternion1">The first source Quaternion.</param>
/// <param name="quaternion2">The second source Quaternion.</param>
/// <param name="amount">The relative weight of the second source Quaternion in the interpolation.</param>
/// <returns>The interpolated Quaternion.</returns>
public static Quaternion Slerp(Quaternion quaternion1, Quaternion quaternion2, Single amount)
{
Single t = amount;
Single cosOmega = quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y +
quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W;
bool flip = false;
if (cosOmega < 0.0f)
{
flip = true;
cosOmega = -cosOmega;
}
Single s1, s2;
if (cosOmega > (1.0f - SlerpEpsilon))
{
// Too close, do straight linear interpolation.
s1 = 1.0f - t;
s2 = (flip) ? -t : t;
}
else
{
Single omega = MathF.Acos(cosOmega);
Single invSinOmega = 1 / MathF.Sin(omega);
s1 = MathF.Sin((1.0f - t) * omega) * invSinOmega;
s2 = (flip)
? -MathF.Sin(t * omega) * invSinOmega
: MathF.Sin(t * omega) * invSinOmega;
}
Quaternion ans;
ans.X = s1 * quaternion1.X + s2 * quaternion2.X;
ans.Y = s1 * quaternion1.Y + s2 * quaternion2.Y;
ans.Z = s1 * quaternion1.Z + s2 * quaternion2.Z;
ans.W = s1 * quaternion1.W + s2 * quaternion2.W;
return(ans);
}
public static Single Distance(Vector3 value1, Vector3 value2)
{
if (Vector.IsHardwareAccelerated)
{
Vector3 difference = value1 - value2;
Single ls = Vector3.Dot(difference, difference);
return(MathF.Sqrt(ls));
}
else
{
Single dx = value1.X - value2.X;
Single dy = value1.Y - value2.Y;
Single dz = value1.Z - value2.Z;
Single ls = dx * dx + dy * dy + dz * dz;
return(MathF.Sqrt(ls));
}
}
public void QuaternionLerpTest()
{
Vector3 axis = Vector3.Normalize(new Vector3(1.0f, 2.0f, 3.0f));
Quaternion a = Quaternion.CreateFromAxisAngle(axis, MathHelper.ToRadians(10.0f));
Quaternion b = Quaternion.CreateFromAxisAngle(axis, MathHelper.ToRadians(30.0f));
Single t = 0.5f;
Quaternion expected = Quaternion.CreateFromAxisAngle(axis, MathHelper.ToRadians(20.0f));
Quaternion actual;
actual = Quaternion.Lerp(a, b, t);
Assert.True(MathHelper.Equal(expected, actual), $"Quaternion.Lerp did not return the expected value: expected {expected} actual {actual}");
// Case a and b are same.
expected = a;
actual = Quaternion.Lerp(a, a, t);
Assert.True(MathHelper.Equal(expected, actual), $"Quaternion.Lerp did not return the expected value: expected {expected} actual {actual}");
}
public PulleyJointDef()
{
JointType = JointType.PulleyJoint;
GroundAnchorA.Set(-1.0f, 1.0f);
GroundAnchorB.Set(1.0f, 1.0f);
LocalAnchorA.Set(-1.0f, 0.0f);
LocalAnchorB.Set(1.0f, 0.0f);
LengthA = 0.0f;
LengthB = 0.0f;
Ratio = 1.0f;
CollideConnected = true;
}
/// <summary>
/// Attempts to invert the given matrix. If the operation succeeds, the inverted matrix is stored in the result parameter.
/// </summary>
/// <param name="matrix">The source matrix.</param>
/// <param name="result">The output matrix.</param>
/// <returns>True if the operation succeeded, False otherwise.</returns>
public static bool Invert(Matrix3x2 matrix, out Matrix3x2 result)
{
Single det = (matrix.M11 * matrix.M22) - (matrix.M21 * matrix.M12);
if (MathF.Abs(det) < Single.Epsilon)
{
result = new Matrix3x2(Single.NaN, Single.NaN, Single.NaN, Single.NaN, Single.NaN, Single.NaN);
return(false);
}
Single invDet = 1.0f / det;
result.M11 = matrix.M22 * invDet;
result.M12 = -matrix.M12 * invDet;
result.M21 = -matrix.M21 * invDet;
result.M22 = matrix.M11 * invDet;
result.M31 = (matrix.M21 * matrix.M32 - matrix.M31 * matrix.M22) * invDet;
result.M32 = (matrix.M31 * matrix.M12 - matrix.M11 * matrix.M32) * invDet;
return(true);
}
public static Vector3 Transform(Vector3 value, Quaternion rotation)
{
Single x2 = rotation.X + rotation.X;
Single y2 = rotation.Y + rotation.Y;
Single z2 = rotation.Z + rotation.Z;
Single wx2 = rotation.W * x2;
Single wy2 = rotation.W * y2;
Single wz2 = rotation.W * z2;
Single xx2 = rotation.X * x2;
Single xy2 = rotation.X * y2;
Single xz2 = rotation.X * z2;
Single yy2 = rotation.Y * y2;
Single yz2 = rotation.Y * z2;
Single zz2 = rotation.Z * z2;
return(new Vector3(
value.X * (1.0f - yy2 - zz2) + value.Y * (xy2 - wz2) + value.Z * (xz2 + wy2),
value.X * (xy2 + wz2) + value.Y * (1.0f - xx2 - zz2) + value.Z * (yz2 - wx2),
value.X * (xz2 - wy2) + value.Y * (yz2 + wx2) + value.Z * (1.0f - xx2 - yy2)));
}
public static Vector3 Clamp(Vector3 value1, Vector3 min, Vector3 max)
{
// This compare order is very important!!!
// We must follow HLSL behavior in the case user specified min value is bigger than max value.
Single x = value1.X;
x = (min.X > x) ? min.X : x; // max(x, minx)
x = (max.X < x) ? max.X : x; // min(x, maxx)
Single y = value1.Y;
y = (min.Y > y) ? min.Y : y; // max(y, miny)
y = (max.Y < y) ? max.Y : y; // min(y, maxy)
Single z = value1.Z;
z = (min.Z > z) ? min.Z : z; // max(z, minz)
z = (max.Z < z) ? max.Z : z; // min(z, maxz)
return(new Vector3(x, y, z));
}
public void QuaternionFromRotationMatrixTest5()
{
for (Single angle = 0.0f; angle < 720.0f; angle += 10.0f)
{
Matrix4x4 matrix = Matrix4x4.CreateRotationX(angle) * Matrix4x4.CreateRotationY(angle) * Matrix4x4.CreateRotationZ(angle);
Quaternion expected =
Quaternion.CreateFromAxisAngle(Vector3.UnitZ, angle) *
Quaternion.CreateFromAxisAngle(Vector3.UnitY, angle) *
Quaternion.CreateFromAxisAngle(Vector3.UnitX, angle);
Quaternion actual = Quaternion.CreateFromRotationMatrix(matrix);
Assert.True(MathHelper.EqualRotation(expected, actual),
$"Quaternion.CreateFromRotationMatrix angle:{angle} did not return the expected value: expected {expected} actual {actual}");
// make sure convert back to matrix is same as we passed matrix.
Matrix4x4 m2 = Matrix4x4.CreateFromQuaternion(actual);
Assert.True(MathHelper.Equal(matrix, m2),
$"Quaternion.CreateFromQuaternion angle:{angle} did not return the expected value: matrix {matrix} m2 {m2}");
}
}
public void Vector3CopyToTest()
{
Vector3 v1 = new Vector3(2.0f, 3.0f, 3.3f);
Single[] a = new Single[4];
Single[] b = new Single[3];
Assert.Throws <NullReferenceException>(() => v1.CopyTo(null, 0));
Assert.Throws <ArgumentOutOfRangeException>(() => v1.CopyTo(a, -1));
Assert.Throws <ArgumentOutOfRangeException>(() => v1.CopyTo(a, a.Length));
AssertExtensions.Throws <ArgumentException>(null, () => v1.CopyTo(a, a.Length - 2));
v1.CopyTo(a, 1);
v1.CopyTo(b);
Assert.Equal(0.0f, a[0]);
Assert.Equal(2.0f, a[1]);
Assert.Equal(3.0f, a[2]);
Assert.Equal(3.3f, a[3]);
Assert.Equal(2.0f, b[0]);
Assert.Equal(3.0f, b[1]);
Assert.Equal(3.3f, b[2]);
}
public static Plane Transform(Plane plane, Quaternion rotation)
{
// Compute rotation matrix.
Single x2 = rotation.X + rotation.X;
Single y2 = rotation.Y + rotation.Y;
Single z2 = rotation.Z + rotation.Z;
Single wx2 = rotation.W * x2;
Single wy2 = rotation.W * y2;
Single wz2 = rotation.W * z2;
Single xx2 = rotation.X * x2;
Single xy2 = rotation.X * y2;
Single xz2 = rotation.X * z2;
Single yy2 = rotation.Y * y2;
Single yz2 = rotation.Y * z2;
Single zz2 = rotation.Z * z2;
Single m11 = 1.0f - yy2 - zz2;
Single m21 = xy2 - wz2;
Single m31 = xz2 + wy2;
Single m12 = xy2 + wz2;
Single m22 = 1.0f - xx2 - zz2;
Single m32 = yz2 - wx2;
Single m13 = xz2 - wy2;
Single m23 = yz2 + wx2;
Single m33 = 1.0f - xx2 - yy2;
Single x = plane.Normal.X, y = plane.Normal.Y, z = plane.Normal.Z;
return(new Plane(
x * m11 + y * m21 + z * m31,
x * m12 + y * m22 + z * m32,
x * m13 + y * m23 + z * m33,
plane.D));
}
/// <summary>
/// Divides a Quaternion by another Quaternion.
/// </summary>
/// <param name="value1">The source Quaternion.</param>
/// <param name="value2">The divisor.</param>
/// <returns>The result of the division.</returns>
public static Quaternion operator /(Quaternion value1, Quaternion value2)
{
Quaternion ans;
Single q1x = value1.X;
Single q1y = value1.Y;
Single q1z = value1.Z;
Single q1w = value1.W;
//-------------------------------------
// Inverse part.
Single ls = value2.X * value2.X + value2.Y * value2.Y +
value2.Z * value2.Z + value2.W * value2.W;
Single invNorm = 1.0f / ls;
Single q2x = -value2.X * invNorm;
Single q2y = -value2.Y * invNorm;
Single q2z = -value2.Z * invNorm;
Single q2w = value2.W * invNorm;
//-------------------------------------
// Multiply part.
// cross(av, bv)
Single cx = q1y * q2z - q1z * q2y;
Single cy = q1z * q2x - q1x * q2z;
Single cz = q1x * q2y - q1y * q2x;
Single dot = q1x * q2x + q1y * q2y + q1z * q2z;
ans.X = q1x * q2w + q2x * q1w + cx;
ans.Y = q1y * q2w + q2y * q1w + cy;
ans.Z = q1z * q2w + q2z * q1w + cz;
ans.W = q1w * q2w - dot;
return(ans);
}
/// <summary>
/// Linearly interpolates between two quaternions.
/// </summary>
/// <param name="quaternion1">The first source Quaternion.</param>
/// <param name="quaternion2">The second source Quaternion.</param>
/// <param name="amount">The relative weight of the second source Quaternion in the interpolation.</param>
/// <returns>The interpolated Quaternion.</returns>
public static Quaternion Lerp(Quaternion quaternion1, Quaternion quaternion2, Single amount)
{
Single t = amount;
Single t1 = 1.0f - t;
Quaternion r = new Quaternion();
Single dot = quaternion1.X * quaternion2.X + quaternion1.Y * quaternion2.Y +
quaternion1.Z * quaternion2.Z + quaternion1.W * quaternion2.W;
if (dot >= 0.0f)
{
r.X = t1 * quaternion1.X + t * quaternion2.X;
r.Y = t1 * quaternion1.Y + t * quaternion2.Y;
r.Z = t1 * quaternion1.Z + t * quaternion2.Z;
r.W = t1 * quaternion1.W + t * quaternion2.W;
}
else
{
r.X = t1 * quaternion1.X - t * quaternion2.X;
r.Y = t1 * quaternion1.Y - t * quaternion2.Y;
r.Z = t1 * quaternion1.Z - t * quaternion2.Z;
r.W = t1 * quaternion1.W - t * quaternion2.W;
}
// Normalize it.
Single ls = r.X * r.X + r.Y * r.Y + r.Z * r.Z + r.W * r.W;
Single invNorm = 1.0f / MathF.Sqrt(ls);
r.X *= invNorm;
r.Y *= invNorm;
r.Z *= invNorm;
r.W *= invNorm;
return(r);
}
public void Matrix3x2CreateRotationRightAngleCenterTest()
{
Vector2 center = new Vector2(3, 7);
// 90 degree rotations must be exact!
Matrix3x2 actual = Matrix3x2.CreateRotation(0, center);
Assert.Equal(new Matrix3x2(1, 0, 0, 1, 0, 0), actual);
actual = Matrix3x2.CreateRotation(MathHelper.Pi / 2, center);
Assert.Equal(new Matrix3x2(0, 1, -1, 0, 10, 4), actual);
actual = Matrix3x2.CreateRotation(MathHelper.Pi, center);
Assert.Equal(new Matrix3x2(-1, 0, 0, -1, 6, 14), actual);
actual = Matrix3x2.CreateRotation(MathHelper.Pi * 3 / 2, center);
Assert.Equal(new Matrix3x2(0, -1, 1, 0, -4, 10), actual);
actual = Matrix3x2.CreateRotation(MathHelper.Pi * 2, center);
Assert.Equal(new Matrix3x2(1, 0, 0, 1, 0, 0), actual);
actual = Matrix3x2.CreateRotation(MathHelper.Pi * 5 / 2, center);
Assert.Equal(new Matrix3x2(0, 1, -1, 0, 10, 4), actual);
actual = Matrix3x2.CreateRotation(-MathHelper.Pi / 2, center);
Assert.Equal(new Matrix3x2(0, -1, 1, 0, -4, 10), actual);
// But merely close-to-90 rotations should not be excessively clamped.
Single delta = MathHelper.ToRadians(0.01f);
actual = Matrix3x2.CreateRotation(MathHelper.Pi + delta, center);
Assert.False(MathHelper.Equal(new Matrix3x2(-1, 0, 0, -1, 6, 14), actual));
actual = Matrix3x2.CreateRotation(MathHelper.Pi - delta, center);
Assert.False(MathHelper.Equal(new Matrix3x2(-1, 0, 0, -1, 6, 14), actual));
}
internal PrismaticJoint(PrismaticJointDef def) : base(def)
{
LocalAnchorA = def.LocalAnchorA;
LocalAnchorB = def.LocalAnchorB;
LocalXAxisA = def.LocalAxisA;
LocalXAxisA.Normalize();
_localYAxisA = MathUtils.Cross(1.0f, LocalXAxisA);
ReferenceAngle = def.ReferenceAngle;
_impulse.SetZero();
_motorMass = 0.0f;
_motorImpulse = 0.0f;
_lowerTranslation = def.LowerTranslation;
_upperTranslation = def.UpperTranslation;
_maxMotorForce = def.MaxMotorForce;
_motorSpeed = def.MotorSpeed;
_enableLimit = def.EnableLimit;
_enableMotor = def.EnableMotor;
_limitState = LimitState.InactiveLimit;
_axis.SetZero();
_perp.SetZero();
}
/// <summary>
/// Constructs a Plane from the given Vector4.
/// </summary>
/// <param name="value">A vector whose first 3 elements describe the normal vector,
/// and whose W component defines the distance along that normal from the origin.</param>
public Fix64Plane(Vector4 value)
{
Normal = new Vector3(value.X, value.Y, value.Z);
D = value.W;
}
// Comparison helpers with small tolerance to allow for floating point rounding during computations.
public static bool Equal(Single a, Single b)
{
return(Math.Abs(a - b) < 1e-5);
}
// Angle conversion helper.
public static Single ToRadians(Single degrees)
{
return(degrees * Pi / 180f);
}
/// <summary>
/// Constructs a Plane from the X, Y, and Z components of its normal, and its distance from the origin on that normal.
/// </summary>
/// <param name="x">The X-component of the normal.</param>
/// <param name="y">The Y-component of the normal.</param>
/// <param name="z">The Z-component of the normal.</param>
/// <param name="d">The distance of the Plane along its normal from the origin.</param>
public Fix64Plane(Single x, Single y, Single z, Single d)
{
Normal = new Vector3(x, y, z);
this.D = d;
}
/// <inheritdoc />
public override Single GetReactionTorque(Single inv_dt)
{
return(inv_dt * _impulse.Y);
}
/// <inheritdoc />
public override Vector2 GetReactionForce(Single inv_dt)
{
return(inv_dt * (_impulse.X * _perp + (_motorImpulse + _impulse.Z) * _axis));
}
/// Get the current motor force given the inverse time step, usually in N.
private Single GetMotorForce(Single inv_dt)
{
return(inv_dt * _motorImpulse);
}
public Fix64Vector3(Single x, Single y, Single z)
{
X = x;
Y = y;
Z = z;
}
public Fix64Vector3(Vector2 value, Single z) : this(value.X, value.Y, z)
{
}
public Fix64Vector3(Single value) : this(value, value, value)
{
}
/// <inheritdoc />
public override Single GetReactionTorque(Single inv_dt)
{
return(0.0f);
}
/// <summary>
/// Constructs a Plane from the given normal and distance along the normal from the origin.
/// </summary>
/// <param name="normal">The Plane's normal vector.</param>
/// <param name="d">The Plane's distance from the origin along its normal vector.</param>
public Fix64Plane(Vector3 normal, Single d)
{
this.Normal = normal;
this.D = d;
}
/// Get the reaction force on bodyB at the joint anchor in Newtons. public abstract Vector2 GetReactionForce(Single inv_dt);
/// Get the reaction torque on bodyB in N*m. public abstract Single GetReactionTorque(Single inv_dt);
/// <inheritdoc />
public override Vector2 GetReactionForce(Single inv_dt)
{
var P = _impulse * _uB;
return(inv_dt * P);
}