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