/// <summary>
/// Calculates angle.
/// </summary>
/// <returns>The angle.</returns>
/// <param name="a">The alpha component.</param>
/// <param name="b">The blue component.</param>
/// <param name="normal">Normal.</param>
public static double CalculateAngle(
Vector3d a,
Vector3d b,
Vector3d normal)
{
Vector3d c = a.Cross(b);
double angle = Math.Atan2(c.Length(), a.Dot(b));
return(c.Dot(normal) < 0.0 ? -angle : angle);
}
public static Matrix3x3 GetRotationMatrix(Vector3d a, Vector3d b)
{
a = a.Normalize();
b = b.Normalize();
if (a != b &&
a.Length() > 0 &&
b.Length() > 0)
{
Vector3d c = a.Cross(b);
return(new Matrix3x3(
a.x, b.x, c.x,
a.y, b.y, c.y,
a.z, b.z, c.z));
}
return(new Matrix3x3());
}
/// <summary>
/// Tests if point inside polygon.
/// </summary>
/// <returns>The point inside polygon.</returns>
/// <param name="vertex">Vertex.</param>
/// <param name="point">Point.</param>
/// <param name="planeNormal">Plane normal.</param>
/// <param name="referencePoint">Reference point.</param>
public static double TestPointInsidePolygon(
Vector3d[] vertex,
Vector3d point,
Vector3d planeNormal,
Vector3d referencePoint)
{
double m1 = 0.0;
double m2 = 0.0;
double anglesum = 0.0;
double costheta = 0.0;
for (int i = 0; i < vertex.Length; i++)
{
Vector3d projectP1 = vertex[i] -
(planeNormal * (Vector3d.Dot(planeNormal, vertex[i]) +
Vector3d.Dot(planeNormal * -1.0, referencePoint)));
Vector3d projectP2 = vertex[(i + 1) % vertex.Length] -
(planeNormal * (Vector3d.Dot(planeNormal, vertex[(i + 1) % vertex.Length]) +
Vector3d.Dot(planeNormal * -1.0, referencePoint)));
Vector3d p1 = projectP1 - point;
Vector3d p2 = projectP2 - point;
m1 = p1.Length();
m2 = p2.Length();
double prod = m1 * m2;
//We are on a node, consider this inside
if (prod <= ConstValues.precision)
{
return(2.0 * ConstValues.PI);
}
else
{
costheta = p1.Dot(p2) / prod;
}
anglesum += Math.Acos(costheta);
}
return(anglesum);
}
/// <summary>
/// Gets the projected point on line.
/// </summary>
/// <returns>The projected point on line.</returns>
/// <param name="a">The alpha component.</param>
/// <param name="b">The blue component.</param>
/// <param name="p">P.</param>
/// <param name="t">T.</param>
public static Vector3d GetProjectedPointOnLine(
Vector3d a,
Vector3d b,
Vector3d p,
ref double t)
{
Vector3d c = b - a;
Vector3d d = p - a;
double mod = Vector3d.Length(c);
t = c.Dot(d) / (mod * mod);
if (t < 0.0)
{
return(a);
}
if (t > 1.0)
{
return(b);
}
return(a + (c * t));
}