/// <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)); }