/// <summary>
/// Determines whether the given sphere intersects the current instance of <see cref="ConeF"/>.
/// </summary>
/// <param name="sphere">The sphere to check.</param>
/// <returns>True if the given sphere intersects the current instance of <see cref="ConeF"/>; False otherwise.</returns>
public bool Intersects(SphereF sphere)
{
float sinAngle = MathEx.Sin(Angle);
float cosAngle = MathEx.Cos(Angle);
float invSin = 1.0f / sinAngle;
float cosSqr = cosAngle * cosAngle;
Vector3F cmv = sphere.Origin - Origin;
Vector3F d = cmv + (sphere.Radius * invSin) * Axis;
float lengthSqr = d.LengthSquared();
float e = Vector3F.Dot(d, Axis);
if (e > 0 && e * e >= lengthSqr * cosSqr)
{
float sinSqr = sinAngle * sinAngle;
lengthSqr = cmv.LengthSquared();
e = -Vector3F.Dot(cmv, Axis);
if (e > 0.0f && e * e >= lengthSqr * sinSqr)
{
float rSqr = sphere.Radius * sphere.Radius;
return(lengthSqr <= rSqr);
}
return(true);
}
return(false);
}
public bool Intersects(RayF ray)
{
Vector3F p = ray.Origin - Origin;
double a = Vector3F.Dot(ray.Direction, ray.Direction);
double b = Vector3F.Dot(ray.Direction, p);
double c = Vector3F.Dot(p, p) - Radius * Radius;
double d = b * b - c * a;
if (d < 0)
{
return(false);
}
return(true);
}
public void ToBounds(out BoundsF result)
{
Vector3F a = Point2 - Point1;
var dotA = Vector3F.Dot(a, a);
if (dotA == 0)
{
dotA = MathEx.Epsilon;
}
Vector3F e = new Vector3F(
Radius * MathEx.Sqrt(1.0f - a.X * a.X / dotA),
Radius * MathEx.Sqrt(1.0f - a.Y * a.Y / dotA),
Radius * MathEx.Sqrt(1.0f - a.Z * a.Z / dotA));
result = new BoundsF(Vector3F.Min(Point1 - e, Point2 - e), Vector3F.Max(Point1 + e, Point2 + e));
}
public bool Intersects(RayF ray, out float scale1, out float scale2)
{
Vector3F p = ray.Origin - Origin;
double a = Vector3F.Dot(ray.Direction, ray.Direction);
double b = Vector3F.Dot(ray.Direction, p);
double c = Vector3F.Dot(p, p) - Radius * Radius;
double d = b * b - c * a;
if (d < 0)
{
scale1 = 0;
scale2 = 0;
return(false);
}
double sqrt = Math.Sqrt(d);
a = 1.0f / a;
scale1 = (float)((-b + sqrt) * a);
scale2 = (float)((-b - sqrt) * a);
return(true);
}
public bool Intersects(Line3F line)
{
Vector3F s = line.Start - Origin;
Vector3F e = line.End - Origin;
Vector3F r = e - s;
float a = Vector3F.Dot(-s, r);
if (a <= 0)
{
return(Vector3F.Dot(s, s) < Radius * Radius);
}
else if (a >= Vector3F.Dot(r, r))
{
return(Vector3F.Dot(e, e) < Radius * Radius);
}
else
{
r = s + (a / (Vector3F.Dot(r, r))) * r;
return(Vector3F.Dot(r, r) < Radius * Radius);
}
}
//!!!!было. сложно.
//public bool Add( Vec3F p )
//{
// if( radius < 0.0f )
// {
// origin = p;
// radius = 0.0f;
// return true;
// }
// else
// {
// float x = p.x - origin.x;
// float y = p.y - origin.y;
// float z = p.z - origin.z;
// float lengthSqr = x * x + y * y + z * z;
// float r = lengthSqr;
// //float r = ( p - origin ).LengthSqr();
// if( r > radius * radius )
// {
// r = MathEx.Sqrt( r );
// float coef = .5f * ( 1.0f - radius / r );
// origin.x += x * coef;
// origin.y += y * coef;
// origin.z += z * coef;
// //origin += ( p - origin ) * 0.5f * ( 1.0f - radius / r );
// radius += .5f * ( r - radius );
// return true;
// }
// return false;
// }
//}
static void ClosestPtPointTriangle(ref Vector3F p, ref Vector3F a, ref Vector3F b, ref Vector3F c, out Vector3F result)
{
Vector3F ab;
Vector3F.Subtract(ref b, ref a, out ab);
//Vec3 ab = b - a;
Vector3F ac;
Vector3F.Subtract(ref c, ref a, out ac);
//Vec3 ac = c - a;
Vector3F ap;
Vector3F.Subtract(ref p, ref a, out ap);
//Vec3 ap = p - a;
float d1;
Vector3F.Dot(ref ab, ref ap, out d1);
float d2;
Vector3F.Dot(ref ac, ref ap, out d2);
if (d1 <= 0.0f && d2 <= 0.0f)
{
result = a;
return;
}
//check if P in vertex region outside B
Vector3F bp;
Vector3F.Subtract(ref p, ref b, out bp);
//Vec3 bp = p - b;
float d3;
Vector3F.Dot(ref ab, ref bp, out d3);
float d4;
Vector3F.Dot(ref ac, ref bp, out d4);
if (d3 >= 0.0f && d4 <= d3)
{
result = b;
return;
}
float vc = d1 * d4 - d3 * d2;
if (vc <= 0.0f && d1 >= 0.0f && d3 <= 0.0f)
{
float v = d1 / (d1 - d3);
result = a + v * ab;
return;
}
Vector3F cp;
Vector3F.Subtract(ref p, ref c, out cp);
//Vec3 cp = p - c;
float d5; Vector3F.Dot(ref ab, ref cp, out d5);
float d6; Vector3F.Dot(ref ac, ref cp, out d6);
if (d6 >= 0.0f && d5 <= d6)
{
result = c;
return;
}
float vb = d5 * d2 - d1 * d6;
if (vb <= 0.0f && d2 >= 0.0f && d6 <= 0.0f)
{
float w = d2 / (d2 - d6);
result = a + w * ac;
return;
}
float va = d3 * d6 - d5 * d4;
if (va <= 0.0f && (d4 - d3) >= 0.0f && (d5 - d6) >= 0.0f)
{
float w = (d4 - d3) / ((d4 - d3) + (d5 - d6));
result = b + w * (c - b);
return;
}
{
float denom = 1.0f / (va + vb + vc);
float v = vb * denom;
float w = vc * denom;
result = a + ab * v + ac * w;
return;
}
}
/// <summary>
/// Creates an instance of <see cref="PlaneF"/> with the normal and the point.
/// </summary>
/// <param name="point">Any point that lies along the plane.</param>
/// <param name="normal">The normal vector to the plane.</param>
/// <param name="plane">When the method completes, contains the resulting plane.</param>
public static void FromPointAndNormal(ref Vector3F point, ref Vector3F normal, out PlaneF plane)
{
float distance = Vector3F.Dot(ref point, ref normal);
plane = new PlaneF(normal, distance);
}
/// <summary>
/// Creates an instance of <see cref="PlaneF"/> with the normal and the point.
/// </summary>
/// <param name="point">Any point that lies along the plane.</param>
/// <param name="normal">The normal vector to the plane.</param>
/// <returns>The resulting plane.</returns>
public static PlaneF FromPointAndNormal(Vector3F point, Vector3F normal)
{
float distance = Vector3F.Dot(ref point, ref normal);
return(new PlaneF(normal, distance));
}
