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