public double AngleTo(Vector3 v) { var theta = this.Dot(v) / (_Math.Sqrt(this.LengthSq() * v.LengthSq())); // clamp, to handle numerical problems return(_Math.Cos(Math.Clamp(theta, -1, 1))); }
public Matrix4 LookAt(Vector3 eye, Vector3 target, Vector3 up) { var x = new Vector3(); var y = new Vector3(); var z = new Vector3(); var te = this.elements; z.SubVectors(eye, target); if (z.LengthSq() == 0) { // eye and target are in the same position z.z = 1; } z.Normalize(); x.CrossVectors(up, z); if (x.LengthSq() == 0) { // up and z are parallel if (_Math.Abs(up.z) == 1) { z.x += 0.0001; } else { z.z += 0.0001; } z.Normalize(); x.CrossVectors(up, z); } x.Normalize(); y.CrossVectors(z, x); te[0] = x.x; te[4] = y.x; te[8] = z.x; te[1] = x.y; te[5] = y.y; te[9] = z.y; te[2] = x.z; te[6] = y.z; te[10] = z.z; return(this); }
public Vector3 ProjectOnVector(Vector3 vector) { var scalar = vector.Dot(this) / vector.LengthSq(); return(this.Copy(vector).MultiplyScalar(scalar)); }
public double DistanceSqToSegment(Vector3 v0, Vector3 v1, Vector3 optionalPointOnRay = null, Vector3 optionalPointOnSegment = null) { var segCenter = new Vector3(); var segDir = new Vector3(); var diff = new Vector3(); // from http://www.geometrictools.com/GTEngine/Include/Mathematics/GteDistRaySegment.h // It returns the min distance between the ray and the segment // defined by v0 and v1 // It can also set two optional targets : // - The closest point on the ray // - The closest point on the segment segCenter.Copy(v0).Add(v1).MultiplyScalar(0.5); segDir.Copy(v1).Sub(v0).Normalize(); diff.Copy(this.origin).Sub(segCenter); var segExtent = v0.DistanceTo(v1) * 0.5; var a01 = -this.direction.Dot(segDir); var b0 = diff.Dot(this.direction); var b1 = -diff.Dot(segDir); var c = diff.LengthSq(); var det = _Math.Abs(1 - a01 * a01); double s0, s1, sqrDist, extDet; if (det > 0) { // The ray and segment are not parallel. s0 = a01 * b1 - b0; s1 = a01 * b0 - b1; extDet = segExtent * det; if (s0 >= 0) { if (s1 >= -extDet) { if (s1 <= extDet) { // region 0 // Minimum at interior points of ray and segment. var invDet = 1 / det; s0 *= invDet; s1 *= invDet; sqrDist = s0 * (s0 + a01 * s1 + 2 * b0) + s1 * (a01 * s0 + s1 + 2 * b1) + c; } else { // region 1 s1 = segExtent; s0 = _Math.Max(0, -(a01 * s1 + b0)); sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c; } } else { // region 5 s1 = -segExtent; s0 = _Math.Max(0, -(a01 * s1 + b0)); sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c; } } else { if (s1 <= -extDet) { // region 4 s0 = _Math.Max(0, -(-a01 * segExtent + b0)); s1 = (s0 > 0) ? -segExtent : _Math.Min(_Math.Max(-segExtent, -b1), segExtent); sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c; } else if (s1 <= extDet) { // region 3 s0 = 0; s1 = _Math.Min(_Math.Max(-segExtent, -b1), segExtent); sqrDist = s1 * (s1 + 2 * b1) + c; } else { // region 2 s0 = _Math.Max(0, -(a01 * segExtent + b0)); s1 = (s0 > 0) ? segExtent : _Math.Min(_Math.Max(-segExtent, -b1), segExtent); sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c; } } } else { // Ray and segment are parallel. s1 = (a01 > 0) ? -segExtent : segExtent; s0 = _Math.Max(0, -(a01 * s1 + b0)); sqrDist = -s0 * s0 + s1 * (s1 + 2 * b1) + c; } if (optionalPointOnRay != null) { optionalPointOnRay.Copy(this.direction).MultiplyScalar(s0).Add(this.origin); } if (optionalPointOnSegment != null) { optionalPointOnSegment.Copy(segDir).MultiplyScalar(s1).Add(segCenter); } return(sqrDist); }