public Quaternion SetFromUnitVectors(Vector3 vFrom, Vector3 vTo) { // assumes direction vectors vFrom and vTo are normalized var v1 = new Vector3(); var EPS = 0.000001; double r = vFrom.Dot(vTo) + 1; if (r < EPS) { r = 0; if (_Math.Abs(vFrom.x) > _Math.Abs(vFrom.z)) { v1.Set(-vFrom.y, vFrom.x, 0); } else { v1.Set(0, -vFrom.z, vFrom.y); } } else { v1.CrossVectors(vFrom, vTo); } this._x = v1.x; this._y = v1.y; this._z = v1.z; this._w = r; return(this.Normalize()); }
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 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)); }