public double GetArea() { var v0 = new Vector3(); var v1 = new Vector3(); v0.SubVectors(this.c, this.b); v1.SubVectors(this.a, this.b); return(v0.Cross(v1).Length() * 0.5); }
public Plane SetFromCoplanarPoints(Vector3 a, Vector3 b, Vector3 c) { var v1 = new Vector3(); var v2 = new Vector3(); var normal = v1.SubVectors(c, b).Cross(v2.SubVectors(a, b)).Normalize(); // Q: should an error be thrown if normal is zero (e.g. degenerate plane)? this.SetFromNormalAndCoplanarPoint(normal, a); return(this); }
public double DistanceSqToPoint(Vector3 point) { var v1 = new Vector3(); var directionDistance = v1.SubVectors(point, this.origin).Dot(this.direction); // point behind the ray if (directionDistance < 0) { return(this.origin.DistanceToSquared(point)); } v1.Copy(this.direction).MultiplyScalar(directionDistance).Add(this.origin); return(v1.DistanceToSquared(point)); }
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 double ClosestPointToPointParameter(Vector3 point, bool clampToLine = false) { var startP = new Vector3(); var startEnd = new Vector3(); startP.SubVectors(point, this.start); startEnd.SubVectors(this.end, this.start); var startEnd2 = startEnd.Dot(startEnd); var startEnd_startP = startEnd.Dot(startP); var t = startEnd_startP / startEnd2; if (clampToLine) { t = Math.Clamp(t, 0, 1); } return(t); }
public Vector3 IntersectSphere(Sphere sphere, Vector3 target) { var v1 = new Vector3(); v1.SubVectors(sphere.center, this.origin); var tca = v1.Dot(this.direction); var d2 = v1.Dot(v1) - tca * tca; var radius2 = sphere.radius * sphere.radius; if (d2 > radius2) { return(null); } var thc = _Math.Sqrt(radius2 - d2); // t0 = first intersect point - entrance on front of sphere var t0 = tca - thc; // t1 = second intersect point - exit point on back of sphere var t1 = tca + thc; // test to see if both t0 and t1 are behind the ray - if so, return null if (t0 < 0 && t1 < 0) { return(null); } // test to see if t0 is behind the ray: // if it is, the ray is inside the sphere, so return the second exit point scaled by t1, // in order to always return an intersect point that is in front of the ray. if (t0 < 0) { return(this.At(t1, target)); } // else t0 is in front of the ray, so return the first collision point scaled by t0 return(this.At(t0, target)); }
// static/instance method to calculate barycentric coordinates // based on: http://www.blackpawn.com/texts/pointinpoly/default.html public Vector3 GetBarycoord(Vector3 point, Vector3 a, Vector3 b, Vector3 c, Vector3 target = null) { var v0 = new Vector3(); var v1 = new Vector3(); var v2 = new Vector3(); v0.SubVectors(c, a); v1.SubVectors(b, a); v2.SubVectors(point, a); var dot00 = v0.Dot(v0); var dot01 = v0.Dot(v1); var dot02 = v0.Dot(v2); var dot11 = v1.Dot(v1); var dot12 = v1.Dot(v2); var denom = (dot00 * dot11 - dot01 * dot01); if (target == null) { Console.WriteLine("THREE.Triangle: .getBarycoord() target is now required"); target = new Vector3(); } // collinear or singular triangle if (denom == 0) { // arbitrary location outside of triangle? // not sure if this is the best idea, maybe should be returning undefined return(target.Set(-2, -1, -1)); } var invDenom = 1 / denom; var u = (dot11 * dot02 - dot01 * dot12) * invDenom; var v = (dot00 * dot12 - dot01 * dot02) * invDenom; // barycentric coordinates must always sum to 1 return(target.Set(1 - u - v, v, u)); }
public Vector3 GetNormal(Vector3 a, Vector3 b, Vector3 c, Vector3 target = null) { var v0 = new Vector3(); if (target == null) { Console.WriteLine("THREE.Triangle: .getNormal() target is now required"); target = new Vector3(); } target.SubVectors(c, b); v0.SubVectors(a, b); target.Cross(v0); var targetLengthSq = target.LengthSq(); if (targetLengthSq > 0) { return(target.MultiplyScalar(1 / _Math.Sqrt(targetLengthSq))); } return(target.Set(0, 0, 0)); }
public bool IntersectsTriangle(Triangle triangle) { // triangle centered vertices var v0 = new Vector3(); var v1 = new Vector3(); var v2 = new Vector3(); // triangle edge vectors var f0 = new Vector3(); var f1 = new Vector3(); var f2 = new Vector3(); var testAxis = new Vector3(); var center = new Vector3(); var extents = new Vector3(); var triangleNormal = new Vector3(); if (this.IsEmpty()) { return(false); } // compute box center and extents this.GetCenter(center); extents.SubVectors(this.max, center); // translate triangle to aabb origin v0.SubVectors(triangle.a, center); v1.SubVectors(triangle.b, center); v2.SubVectors(triangle.c, center); // compute edge vectors for triangle f0.SubVectors(v1, v0); f1.SubVectors(v2, v1); f2.SubVectors(v0, v2); // test against axes that are given by cross product combinations of the edges of the triangle and the edges of the aabb // make an axis testing of each of the 3 sides of the aabb against each of the 3 sides of the triangle = 9 axis of separation // axis_ij = u_i x f_j (u0, u1, u2 = face normals of aabb = x,y,z axes vectors since aabb is axis aligned) var axes = new double[] { 0, -f0.z, f0.y, 0, -f1.z, f1.y, 0, -f2.z, f2.y, f0.z, 0, -f0.x, f1.z, 0, -f1.x, f2.z, 0, -f2.x, -f0.y, f0.x, 0, -f1.y, f1.x, 0, -f2.y, f2.x, 0 }; if (!_SatForAxes(axes, testAxis, extents, v0, v1, v2)) { return(false); } // test 3 face normals from the aabb axes = new double[] { 1, 0, 0, 0, 1, 0, 0, 0, 1 }; if (!_SatForAxes(axes, testAxis, extents, v0, v1, v2)) { return(false); } // finally testing the face normal of the triangle // use already existing triangle edge vectors here triangleNormal.CrossVectors(f0, f1); axes = new double[] { triangleNormal.x, triangleNormal.y, triangleNormal.z }; return(_SatForAxes(axes, testAxis, extents, v0, v1, v2)); }
public Vector3 IntersectTriangle(Vector3 a, Vector3 b, Vector3 c, bool backfaceCulling, Vector3 target) { // Compute the offset origin, edges, and normal. var diff = new Vector3(); var edge1 = new Vector3(); var edge2 = new Vector3(); var normal = new Vector3(); // from http://www.geometrictools.com/GTEngine/Include/Mathematics/GteIntrRay3Triangle3.h edge1.SubVectors(b, a); edge2.SubVectors(c, a); normal.CrossVectors(edge1, edge2); // Solve Q + t*D = b1*E1 + b2*E2 (Q = kDiff, D = ray direction, // E1 = kEdge1, E2 = kEdge2, N = Cross(E1,E2)) by // |Dot(D,N)|*b1 = sign(Dot(D,N))*Dot(D,Cross(Q,E2)) // |Dot(D,N)|*b2 = sign(Dot(D,N))*Dot(D,Cross(E1,Q)) // |Dot(D,N)|*t = -sign(Dot(D,N))*Dot(Q,N) var DdN = this.direction.Dot(normal); double sign; if (DdN > 0) { if (backfaceCulling) { return(null); } sign = 1; } else if (DdN < 0) { sign = -1; DdN = -DdN; } else { return(null); } diff.SubVectors(this.origin, a); var DdQxE2 = sign * this.direction.Dot(edge2.CrossVectors(diff, edge2)); // b1 < 0, no intersection if (DdQxE2 < 0) { return(null); } var DdE1xQ = sign * this.direction.Dot(edge1.Cross(diff)); // b2 < 0, no intersection if (DdE1xQ < 0) { return(null); } // b1+b2 > 1, no intersection if (DdQxE2 + DdE1xQ > DdN) { return(null); } // Line intersects triangle, check if ray does. var QdN = -sign *diff.Dot(normal); // t < 0, no intersection if (QdN < 0) { return(null); } // Ray intersects triangle. return(this.At(QdN / DdN, target)); }
public Vector3 ClosestPointToPoint(Vector3 p, Vector3 target = null) { var vab = new Vector3(); var vac = new Vector3(); var vbc = new Vector3(); var vap = new Vector3(); var vbp = new Vector3(); var vcp = new Vector3(); if (target == null) { Console.WriteLine("THREE.Triangle: .closestPointToPoint() target is now required"); target = new Vector3(); } Vector3 a = this.a, b = this.b, c = this.c; double v, w; // algorithm thanks to Real-Time Collision Detection by Christer Ericson, // published by Morgan Kaufmann Publishers, (c) 2005 Elsevier Inc., // under the accompanying license; see chapter 5.1.5 for detailed explanation. // basically, we're distinguishing which of the voronoi regions of the triangle // the point lies in with the minimum amount of redundant computation. vab.SubVectors(b, a); vac.SubVectors(c, a); vap.SubVectors(p, a); var d1 = vab.Dot(vap); var d2 = vac.Dot(vap); if (d1 <= 0 && d2 <= 0) { // vertex region of A; barycentric coords (1, 0, 0) return(target.Copy(a)); } vbp.SubVectors(p, b); var d3 = vab.Dot(vbp); var d4 = vac.Dot(vbp); if (d3 >= 0 && d4 <= d3) { // vertex region of B; barycentric coords (0, 1, 0) return(target.Copy(b)); } var vc = d1 * d4 - d3 * d2; if (vc <= 0 && d1 >= 0 && d3 <= 0) { v = d1 / (d1 - d3); // edge region of AB; barycentric coords (1-v, v, 0) return(target.Copy(a).AddScaledVector(vab, v)); } vcp.SubVectors(p, c); var d5 = vab.Dot(vcp); var d6 = vac.Dot(vcp); if (d6 >= 0 && d5 <= d6) { // vertex region of C; barycentric coords (0, 0, 1) return(target.Copy(c)); } var vb = d5 * d2 - d1 * d6; if (vb <= 0 && d2 >= 0 && d6 <= 0) { w = d2 / (d2 - d6); // edge region of AC; barycentric coords (1-w, 0, w) return(target.Copy(a).AddScaledVector(vac, w)); } var va = d3 * d6 - d5 * d4; if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0) { vbc.SubVectors(c, b); w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); // edge region of BC; barycentric coords (0, 1-w, w) return(target.Copy(b).AddScaledVector(vbc, w)); // edge region of BC } // face region var denom = 1 / (va + vb + vc); // u = va * denom v = vb * denom; w = vc * denom; return(target.Copy(a).AddScaledVector(vab, v).AddScaledVector(vac, w)); }