/// <summary>Create a rotation from an axis and angle</summary>
public static m3x4 Rotation(v4 axis_norm, v4 axis_sine_angle, float cos_angle)
{
Debug.Assert(Math_.FEql(axis_norm.LengthSq, 1f), "'axis_norm' should be normalised");
var mat = new m3x4();
v4 trace_vec = axis_norm * (1.0f - cos_angle);
mat.x.x = trace_vec.x * axis_norm.x + cos_angle;
mat.y.y = trace_vec.y * axis_norm.y + cos_angle;
mat.z.z = trace_vec.z * axis_norm.z + cos_angle;
trace_vec.x *= axis_norm.y;
trace_vec.z *= axis_norm.x;
trace_vec.y *= axis_norm.z;
mat.x.y = trace_vec.x + axis_sine_angle.z;
mat.x.z = trace_vec.z - axis_sine_angle.y;
mat.x.w = 0.0f;
mat.y.x = trace_vec.x - axis_sine_angle.z;
mat.y.z = trace_vec.y + axis_sine_angle.x;
mat.y.w = 0.0f;
mat.z.x = trace_vec.z + axis_sine_angle.y;
mat.z.y = trace_vec.y - axis_sine_angle.x;
mat.z.w = 0.0f;
return(mat);
}
/// <summary>Return the closest point between two line segments</summary>
public static void ClosestPoint(v4 s0, v4 e0, v4 s1, v4 e1, out float t0, out float t1)
{
Debug.Assert(s0.w == 1f && e0.w == 1f && s1.w == 1f && e1.w == 1f);
v4 line0 = e0 - s0;
v4 line1 = e1 - s1;
v4 separation = s0 - s1;
float f = Math_.Dot(line1, separation);
float c = Math_.Dot(line0, separation);
float line0_length_sq = line0.LengthSq;
float line1_length_sq = line1.LengthSq;
// Check if either or both segments are degenerate
if (Math_.FEql(line0_length_sq, 0f) && Math_.FEql(line1_length_sq, 0f))
{
t0 = 0.0f; t1 = 0.0f; return;
}
if (Math_.FEql(line0_length_sq, 0f))
{
t0 = 0.0f; t1 = Math_.Clamp(f / line1_length_sq, 0.0f, 1.0f); return;
}
if (Math_.FEql(line1_length_sq, 0f))
{
t1 = 0.0f; t0 = Math_.Clamp(-c / line0_length_sq, 0.0f, 1.0f); return;
}
// The general nondegenerate case starts here
float b = Math_.Dot(line0, line1);
float denom = line0_length_sq * line1_length_sq - b * b; // Always non-negative
// If segments not parallel, calculate closest point on infinite line 'line0'
// to infinite line 'line1', and clamp to segment 1. Otherwise pick arbitrary t0
t0 = denom != 0.0f ? Math_.Clamp((b * f - c * line1_length_sq) / denom, 0.0f, 1.0f) : 0.0f;
// Calculate point on infinite line 'line1' closest to segment 'line0' at t0
// using t1 = Dot3(pt0 - s1, line1) / line1_length_sq = (b*t0 + f) / line1_length_sq
t1 = (b * t0 + f) / line1_length_sq;
// If t1 in [0,1] then done. Otherwise, clamp t1, recompute t0 for the new value
// of t1 using t0 = Dot3(pt1 - s0, line0) / line0_length_sq = (b*t1 - c) / line0_length_sq
// and clamped t0 to [0, 1]
if (t1 < 0.0f)
{
t1 = 0.0f; t0 = Math_.Clamp((-c) / line0_length_sq, 0.0f, 1.0f);
}
else if (t1 > 1.0f)
{
t1 = 1.0f; t0 = Math_.Clamp((b - c) / line0_length_sq, 0.0f, 1.0f);
}
}
/// <summary>Construct a quaternion representing a rotation from 'from' to 'to'</summary>
public quat(v4 from, v4 to)
: this()
{
var d = Math_.Dot(from.xyz, to.xyz);
var s = (float)Math.Sqrt(from.xyz.LengthSq * to.xyz.LengthSq) + d;
var axis = Math_.Cross(from, to);
// vectors are 180 degrees apart
if (Math_.FEql(s, 0))
{
axis = Math_.Perpendicular(to);
s = 0.0f;
}
xyzw = Math_.Normalise(new v4(axis.x, axis.y, axis.z, s));
}
public m3x4(quat quaterion) : this()
{
Debug.Assert(!Math_.FEql(quaterion, quat.Zero), "'quaternion' is a zero quaternion");
var q = quaterion;
var q_lensq = q.LengthSq;
var s = 2.0f / q_lensq;
float xs = q.x * s, ys = q.y * s, zs = q.z * s;
float wx = q.w * xs, wy = q.w * ys, wz = q.w * zs;
float xx = q.x * xs, xy = q.x * ys, xz = q.x * zs;
float yy = q.y * ys, yz = q.y * zs, zz = q.z * zs;
x.x = 1.0f - (yy + zz); y.x = xy - wz; z.x = xz + wy;
x.y = xy + wz; y.y = 1.0f - (xx + zz); z.y = yz - wx;
x.z = xz - wy; y.z = yz + wx; z.z = 1.0f - (xx + yy);
x.w = y.w = z.w = 0.0f;
}
/// <summary>
/// Return the intercept between a 2d line that passes through 'a' and 'b' and another
/// that passes through 'c' and 'd'. Returns true if the lines intersect, false if they don't.
/// Returns the point of intersect in </summary>
public static bool Intersect(v2 a, v2 b, v2 c, v2 d, out v2 intersect)
{
v2 ab = b - a;
v2 cd = d - c;
float denom = ab.x * cd.y - ab.y * cd.x;
if (Math_.FEql(denom, 0.0f))
{
intersect = v2.Zero;
return(false);
}
float e = b.x * a.y - b.y * a.x;
float f = d.x * c.y - d.y * c.x;
intersect.x = (cd.x * e - ab.x * f) / denom;
intersect.y = (cd.y * e - ab.y * f) / denom;
return(true);
}
/// <summary>Create a transform representing the rotation from one vector to another. 'from' and 'to' do not have to be normalised</summary>
public static m3x4 Rotation(v4 from, v4 to)
{
Debug.Assert(!Math_.FEql(from.Length, 0));
Debug.Assert(!Math_.FEql(to.Length, 0));
var len = from.Length * to.Length;
// Find the cosine of the angle between the vectors
var cos_angle = Math_.Dot(from, to) / len;
if (cos_angle >= 1f - Math_.TinyF)
{
return(Identity);
}
if (cos_angle <= Math_.TinyF - 1f)
{
return(Rotation(Math_.Normalise(Math_.Perpendicular(from - to)), (float)Math_.TauBy2));
}
// Axis multiplied by sine of the angle
var axis_sine_angle = Math_.Cross(from, to) / len;
var axis_norm = Math_.Normalise(axis_sine_angle);
return(Rotation(axis_norm, axis_sine_angle, cos_angle));
}
/// <summary>Create from an axis and angle. 'axis' should be normalised</summary>
public static m3x4 Rotation(v4 axis_norm, float angle)
{
Debug.Assert(Math_.FEql(axis_norm.LengthSq, 1f), "'axis_norm' should be normalised");
return(Rotation(axis_norm, axis_norm * (float)Math.Sin(angle), (float)Math.Cos(angle)));
}
public static m4x4 Translation(v4 translation)
{
Debug.Assert(Math_.FEql(translation.w, 1f), "'translation' must be a position vector");
return(new m4x4(m3x4.Identity, translation));
}