public bool IntersectsBounds(Bounds_d box)
{
Vector3_d dirfrac = new Vector3_d {
x = 1.0 / direction.x,
y = 1.0 / direction.y,
z = 1.0 / direction.z
};
double t1 = (box.min.x - origin.x) * dirfrac.x;
double t2 = (box.max.x - origin.x) * dirfrac.x;
double t3 = (box.min.y - origin.y) * dirfrac.y;
double t4 = (box.max.y - origin.y) * dirfrac.y;
double t5 = (box.min.z - origin.z) * dirfrac.z;
double t6 = (box.max.z - origin.z) * dirfrac.z;
double tmin = Math_d.Max(Math_d.Max(Math_d.Min(t1, t2), Math_d.Min(t3, t4)), Math_d.Min(t5, t6));
double tmax = Math_d.Min(Math_d.Min(Math_d.Max(t1, t2), Math_d.Max(t3, t4)), Math_d.Max(t5, t6));
// if tmax < 0, ray (line) is intersecting AABB, but whole AABB is behing us
if (tmax < 0)
{
return(false);
}
// if tmin <= tmax, ray intersects AABB
return(tmin <= tmax);
}
public static Bounds_d FromPoints(Vector3_d v0, Vector3_d v1)
{
Bounds_d newBounds = new Bounds_d();
newBounds.ConformTo(v0, v1);
return(newBounds);
}
public static Bounds_d FromCenterAndSize(Vector3_d center, Vector3_d size)
{
return(new Bounds_d {
_center = center,
size = size
});
}
public static Bounds_d FromCenterAndExtents(Vector3_d center, Vector3_d extents)
{
return(new Bounds_d {
_center = center,
extents = extents
});
}
public Vector4_d(Vector3_d v3)
{
x = v3.x;
y = v3.y;
z = v3.z;
w = 0.0;
}
public Bounds_d(Bounds bounds)
{
_center = bounds.center;
_extents = bounds.extents;
_size = 2.0 * _extents;
_min = _center - _extents;
_max = _center + _extents;
}
public Vector3_d MultiplyPoint3x4(Vector3_d v)
{
Vector3_d vector3d;
vector3d.x = (m00 * v.x + m01 * v.y + m02 * v.z) + m03;
vector3d.y = (m10 * v.x + m11 * v.y + m12 * v.z) + m13;
vector3d.z = (m20 * v.x + m21 * v.y + m22 * v.z) + m23;
return(vector3d);
}
public Vector3_d MultiplyVector(Vector3_d v)
{
Vector3_d vector3d;
vector3d.x = (m00 * v.x + m01 * v.y + m02 * v.z);
vector3d.y = (m10 * v.x + m11 * v.y + m12 * v.z);
vector3d.z = (m20 * v.x + m21 * v.y + m22 * v.z);
return(vector3d);
}
public Vector3_d MultiplyPoint(Vector3_d v)
{
Vector3_d vector3d;
vector3d.x = (m00 * v.x + m01 * v.y + m02 * v.z) + m03;
vector3d.y = (m10 * v.x + m11 * v.y + m12 * v.z) + m13;
vector3d.z = (m20 * v.x + m21 * v.y + m22 * v.z) + m23;
double num = 1f / ((m30 * v.x + m31 * v.y + m32 * v.z) + m33);
vector3d.x *= num;
vector3d.y *= num;
vector3d.z *= num;
return(vector3d);
}
public static Matrix4x4_d Translate(Vector3_d v)
{
return(new Matrix4x4_d {
m00 = 1f,
m01 = 0.0f,
m02 = 0.0f,
m03 = v.x,
m10 = 0.0f,
m11 = 1f,
m12 = 0.0f,
m13 = v.y,
m20 = 0.0f,
m21 = 0.0f,
m22 = 1f,
m23 = v.z,
m30 = 0.0f,
m31 = 0.0f,
m32 = 0.0f,
m33 = 1f
});
}
// http://geomalgorithms.com/a07-_distance.html#dist3D_Segment_to_Segment()
// you an test distance to a point by setting the first two parameters to the same vector (it doesn't work if the point's coordinates are
// supplied in the last two parameters)
public static Vector3_d[] ClosestSegmentToSegmentPoints(Vector3_d line1p0, Vector3_d line1p1, Vector3_d line2p0, Vector3_d line2p1)
{
const double smallNumber = 0.00000001;
if (line1p0 == line1p1 && line2p0 == line2p1)
{
return(new[] { line1p0, line2p0 });
}
Vector3_d u = line1p1 - line1p0;
Vector3_d v = line2p1 - line2p0;
Vector3_d w = line1p0 - line2p0;
double a = Vector3_d.Dot(u, u); // always >= 0
double b = Vector3_d.Dot(u, v);
double c = Vector3_d.Dot(v, v); // always >= 0
double d = Vector3_d.Dot(u, w);
double e = Vector3_d.Dot(v, w);
double D = a * c - b * b; // always >= 0
double sN, sD = D; // sc = sN / sD, default sD = D >= 0
double tN, tD = D; // tc = tN / tD, default tD = D >= 0
// compute the line parameters of the two closest points
if (D < smallNumber) // the lines are almost parallel
{
sN = 0.0; // force using point P0 on segment S1
sD = 1.0; // to prevent possible division by 0.0 later
tN = e;
tD = c;
}
else // get the closest points on the infinite lines
{
sN = (b * e - c * d);
tN = (a * e - b * d);
if (sN < 0.0) // sc < 0 => the s=0 edge is visible
{
sN = 0.0;
tN = e;
tD = c;
}
else if (sN > sD) // sc > 1 => the s=1 edge is visible
{
sN = sD;
tN = e + b;
tD = c;
}
}
if (tN < 0.0) // tc < 0 => the t=0 edge is visible
{
tN = 0.0;
// recompute sc for this edge
if (-d < 0.0)
{
sN = 0.0;
}
else if (-d > a)
{
sN = sD;
}
else
{
sN = -d;
sD = a;
}
}
else if (tN > tD) // tc > 1 => the t=1 edge is visible
{
tN = tD;
// recompute sc for this edge
if ((-d + b) < 0.0)
{
sN = 0;
}
else if ((-d + b) > a)
{
sN = sD;
}
else
{
sN = (-d + b);
sD = a;
}
}
// finally do the division to get sc and tc
var sc = (Abs(sN) < smallNumber ? 0.0 : sN / sD);
var tc = (Abs(tN) < smallNumber ? 0.0 : tN / tD);
return(new[] {
line1p0 + sc * u,
line2p0 + tc * v
});
}
public Ray_d(Vector3_d origin, Vector3_d direction)
{
this.origin = origin;
this.direction = direction.normalized;
}
public Ray_d(Ray ray)
{
origin = ray.origin;
direction = ray.direction;
}
public static double SqrDistance(Vector3_d a, Vector3_d b)
{
return((a - b).sqrMagnitude);
}
public void ExpandToFit(Bounds_d other)
{
ConformTo(Vector3_d.Min(_min, other.min), Vector3_d.Max(_max, other.max));
}
public static double ClosestSegmentToSegmentSqrDistance(Vector3_d line1p0, Vector3_d line1p1, Vector3_d line2p0, Vector3_d line2p1)
{
Vector3_d[] points = ClosestSegmentToSegmentPoints(line1p0, line1p1, line2p0, line2p1);
return(Vector3_d.SqrDistance(points[0], points[1]));
}
public static Vector3_d ClosestSegmentToSegmentMidPoint(Vector3_d line1p0, Vector3_d line1p1, Vector3_d line2p0, Vector3_d line2p1)
{
Vector3_d[] points = ClosestSegmentToSegmentPoints(line1p0, line1p1, line2p0, line2p1);
return((points[0] + points[1]) / 2.0);
}
public void ConformTo(Vector3_d v0, Vector3_d v1)
{
_center = (v0 + v1) / 2.0;
size = Vector3_d.Max(v0, v1) - Vector3_d.Min(v0, v1);
}
