public static bool IsPlaneIntersectingCone(Vect3 planeNormal, Vect3 planePoint, Vect3 conePosition,
Vect3 coneAxis, double coneAxisLength, double coneCosPhi)
{
double len;
//try "bending" axis towards plane normal
Vect3 q = conePosition + coneAxis * coneAxisLength;
Vect3 qx = planeNormal.ProjectOn(coneAxis);
Vect3 p = qx - planeNormal;
if (System.Math.Abs(p.QuadLength() - 0) < Constants.EPS)
{
// axis equals plane normal
p = coneAxis;
len = coneAxisLength;
}
else
{
//bend axis towards plane normal as far as sinPhi allows
p = p.Normalize();
q = q + (p * coneAxisLength * System.Math.Sin(System.Math.Acos(coneCosPhi)));
q -= conePosition;
len = q.Length();
p = q / len;
}
double d = RayPlane.GetHitPointRayPlaneDistance(conePosition, p, planePoint, planeNormal);
return(d < len);
}
public static GameObject DrawTunnelLine(Vect3 startPos, Vect3 endPos, float scalingFactor, float width, float height, GameObject parent = null)
{
GameObject cylinder = GameObject.CreatePrimitive(PrimitiveType.Cylinder);
if (parent)
{
cylinder.transform.parent = parent.transform;
}
startPos.y += height / (2f * scalingFactor);
endPos.y += height / (2f * scalingFactor);
Vect3 localVect = startPos - endPos;
cylinder.transform.position = GetVector3FromVect3(Vect3.MiddlePoint(startPos, endPos));
cylinder.transform.localScale = new Vector3(width / scalingFactor, (float)localVect.Length() / 2, height / scalingFactor);
Vector3 crossedVector = Vector3.Cross(GetVector3FromVect3(localVect), cylinder.transform.right);
if (crossedVector == Vector3.zero)
{
crossedVector = Vector3.Cross(GetVector3FromVect3(localVect), cylinder.transform.forward);
}
cylinder.transform.rotation = Quaternion.LookRotation(crossedVector,
GetVector3FromVect3(localVect));
return(cylinder);
}
public void When_NormalizeSmaller_Expect_Normal()
{
Vect3 vect1 = new Vect3(0.1d, 0.1d, 0);
Vect3 vect2 = new Vect3(1 / Math.Sqrt(2), 1 / Math.Sqrt(2), 0);
vect1 = vect1.GetNormalized();
Assert.AreEqual(vect1, vect2);
Assert.That(Util.NearlyEqual(1, vect1.Length()));
}
public MinimalTriangle(Vect3 normal, Vect3 v1, Vect3 v2, Vect3 v3) : base(v1, normal)
{
V2 = v2;
V3 = v3;
Vect3 avg = new[] { v1, v2, v3 }.Avg();
Vect3 tmp = avg - v3;
Bounds = new Sphere(avg, tmp.Length(), Color.Black);
}
public void When_NormalizeTwice_Expect_Same(double x, double y, double z)
{
Vect3 v1 = new Vect3(x, y, z);
v1 = v1.GetNormalized();
Vect3 v2 = v1.GetNormalized();
Assert.That(Util.NearlyEqual(v1.Length(), 1));
Assert.That(Util.NearlyEqual(v2.Length(), 1));
Assert.AreEqual(v1, v2);
}
public static bool IsAreaIntersectingCone(Vect3 planeNormal, Vect3 planePoint, double areaWidthHalf,
Vect3 conePosition, Vect3 coneAxis, double coneAxisLength,
double coneCosPhi)
{
double len;
//try "bending" axis towards plane normal
Vect3 q = conePosition + coneAxis * coneAxisLength;
Vect3 qx = planeNormal.ProjectOn(coneAxis);
Vect3 p = qx - planeNormal;
if (System.Math.Abs(p.QuadLength() - 0) < Constants.EPS)
{
// axis equals plane normal
p = coneAxis;
len = coneAxisLength;
}
else
{
//bend axis towards plane normal as far as sinPhi allows
p = p.Normalize();
q = q + (p * coneAxisLength * System.Math.Sin(System.Math.Acos(coneCosPhi)));
q -= conePosition;
len = q.Length();
p = q / len;
}
double d = RayPlane.GetHitPointRayPlaneDistance(conePosition, p, planePoint, planeNormal);
if (d < len)
{
//check if Hitpoint is in the +/-width/2 - area of the plane
p = conePosition + p * d;
return
(System.Math.Abs(p.X - planePoint.X) < areaWidthHalf * 2 &&
System.Math.Abs(p.Y - planePoint.Y) < areaWidthHalf * 2 &&
System.Math.Abs(p.Z - planePoint.Z) < areaWidthHalf * 2);
}
return(false);
}
public static double InterpolateTriangleEdge(Vect3 v1, Vect3 v2, Vect3 v3, Vect3 point)
{
Vect3 v23N = v3 - v2;
v23N = v23N.Normalize();
Vect3 v21 = v1 - v2;
// ReSharper disable InconsistentNaming
Vect3 v1o = v21.ProjectOn(v23N); //punkt gegenüber der ecke v1 (o ... opposite)
// ReSharper restore InconsistentNaming
v1o -= v21;
double h1 = v1o.Length(); //höhe auf v1
v1o = v1o / h1; //normieren
Vect3 v1P = point - v1;
Vect3 p1 = v1P.ProjectOn(v1o); //projektion von v1p auf v1hn
return(1 - (p1.Length() / h1));
}
public TriangleMeshModel(List <I3DObject> triangleEdgeData)
{
_triangles = triangleEdgeData.ToArray();
var max = new Vect3
{
X = double.NegativeInfinity,
Y = double.NegativeInfinity,
Z = double.NegativeInfinity
};
var min = new Vect3
{
X = double.PositiveInfinity,
Y = double.PositiveInfinity,
Z = double.PositiveInfinity
};
foreach (MinimalTriangle m in _triangles)
{
Vect3 p = m.GetBoundingSphere().Position;
if (p.X > max.X)
{
max.X = p.X;
}
if (p.Y > max.Y)
{
max.Y = p.Y;
}
if (p.Z > max.Z)
{
max.Z = p.Z;
}
if (p.X < min.X)
{
min.X = p.X;
}
if (p.Y < min.Y)
{
min.Y = p.Y;
}
if (p.Z < min.Z)
{
min.Z = p.Z;
}
}
max = (max + min) / 2;
double radius = 0;
foreach (MinimalTriangle m in _triangles)
{
Vect3 p = m.GetBoundingSphere().Position;
min = p - max;
double dist = m.GetBoundingSphereRadius() + min.Length();
if (dist > radius)
{
radius = dist;
}
}
_bounds = new Sphere(max, radius, Color.Black);
_tree = Octree.BuildTree(_bounds.Position, _triangles);
foreach (MinimalTriangle m in _triangles)
{
m.Purge();
}
}