/// <summary>
/// Calculates the angle of two rays that do not necessarily share a point
/// </summary>
/// <param name="ray1_1">first vector on first ray</param>
/// <param name="ray1_2">second vector on first ray</param>
/// <param name="ray2_1">first vector on second ray</param>
/// <param name="ray2_2">second vector on second ray</param>
/// <returns>angle in radians</returns>
public static double AngleTo(this NUIVector ray1_1, NUIVector ray1_2, NUIVector ray2_1, NUIVector ray2_2)
{
NUIVector diff1 = ray1_2.Minus(ray1_1);
NUIVector diff2 = ray2_2.Minus(ray2_1);
return Math.Acos(diff1.DotProduct(diff2) / diff1.Length() / diff2.Length());
}
/// <summary>
/// Determines if the triangle collides with a specified point
/// </summary>
/// <param name="v">The vector to be checked</param>
/// <returns>true, if there is a collision</returns>
public override bool Collision(Vector v)
{
Vector Normal = v2.Minus(v1).Cross(v3.Minus(v1));
Normal = Normal.DivideBy((float)Normal.Length());
double Dist = Normal.DotProduct(v.Minus(v1));
if (Math.Abs(Dist) < 0.2)
{
//Vector in plane of triangle
//check barycentric coordinates
//push vector on plane
v = v.Minus(Normal.MultiplyWith((float)Dist));
// calculate offset for a greater tolerance
Vector v12, v13, v23;
v12 = v2.Minus(v1); v12.Normalize();
v13 = v3.Minus(v1); v13.Normalize();
v23 = v3.Minus(v2); v23.Normalize();
Vector off1, off2, off3;
off1 = v12.MultiplyWith(-1).Minus(v13); off1.Normalize();
off2 = v12.Minus(v23); off2.Normalize();
off3 = v13.Plus(v23); off3.Normalize();
Vector _v1 = v1.Plus(off1.MultiplyWith(0.2f));
Vector _v2 = v2.Plus(off2.MultiplyWith(0.2f));
Vector _v3 = v3.Plus(off3.MultiplyWith(0.2f));
float s1, s2, s3;
if (Math.Abs(Normal.X) != 1 && Math.Abs(Normal.Y) != 1) // if triangle is not parallel to YZ-plane or XZ-plane
{
s1 = -(-v.Y * _v2.X + v.X * _v2.Y + v.Y * _v3.X - _v2.Y * _v3.X - v.X * _v3.Y + _v2.X * _v3.Y) / (
_v1.Y * _v2.X - _v1.X * _v2.Y - _v1.Y * _v3.X + _v2.Y * _v3.X + _v1.X * _v3.Y - _v2.X * _v3.Y);
s2 = -(v.Y * _v1.X - v.X * _v1.Y - v.Y * _v3.X + _v1.Y * _v3.X + v.X * _v3.Y - _v1.X * _v3.Y) / (
_v1.Y * _v2.X - _v1.X * _v2.Y - _v1.Y * _v3.X + _v2.Y * _v3.X + _v1.X * _v3.Y - _v2.X * _v3.Y);
s3 = -(-(-v.Y + _v1.Y) * (-_v1.X + _v2.X) + (-v.X + _v1.X) * (-_v1.Y + _v2.Y)) / ((-
_v1.Y + _v2.Y) * (-_v1.X + _v3.X) - (-_v1.X + _v2.X) * (-_v1.Y + _v3.Y));
}
else if(Math.Abs(Normal.X) == 1) //parallel to YZ-plane => use Z instead of X coordinates
{
s1 = -(-v.Y * _v2.Z + v.Z * _v2.Y + v.Y * _v3.Z - _v2.Y * _v3.Z - v.Z * _v3.Y + _v2.Z * _v3.Y) / (
_v1.Y * _v2.Z - _v1.Z * _v2.Y - _v1.Y * _v3.Z + _v2.Y * _v3.Z + _v1.Z * _v3.Y - _v2.Z * _v3.Y);
s2 = -(v.Y * _v1.Z - v.Z * _v1.Y - v.Y * _v3.Z + _v1.Y * _v3.Z + v.Z * _v3.Y - _v1.Z * _v3.Y) / (
_v1.Y * _v2.Z - _v1.Z * _v2.Y - _v1.Y * _v3.Z + _v2.Y * _v3.Z + _v1.Z * _v3.Y - _v2.Z * _v3.Y);
s3 = -(-(-v.Y + _v1.Y) * (-_v1.Z + _v2.Z) + (-v.Z + _v1.Z) * (-_v1.Y + _v2.Y)) / ((-
_v1.Y + _v2.Y) * (-_v1.Z + _v3.Z) - (-_v1.Z + _v2.Z) * (-_v1.Y + _v3.Y));
}
else //parallel to XZ-plane => use Z instead of Y coordinates
{
s1 = -(-v.Z * _v2.X + v.X * _v2.Z + v.Z * _v3.X - _v2.Z * _v3.X - v.X * _v3.Z + _v2.X * _v3.Z) / (
_v1.Z * _v2.X - _v1.X * _v2.Z - _v1.Z * _v3.X + _v2.Z * _v3.X + _v1.X * _v3.Z - _v2.X * _v3.Z);
s2 = -(v.Z * _v1.X - v.X * _v1.Z - v.Z * _v3.X + _v1.Z * _v3.X + v.X * _v3.Z - _v1.X * _v3.Z) / (
_v1.Z * _v2.X - _v1.X * _v2.Z - _v1.Z * _v3.X + _v2.Z * _v3.X + _v1.X * _v3.Z - _v2.X * _v3.Z);
s3 = -(-(-v.Z + _v1.Z) * (-_v1.X + _v2.X) + (-v.X + _v1.X) * (-_v1.Z + _v2.Z)) / ((-
_v1.Z + _v2.Z) * (-_v1.X + _v3.X) - (-_v1.X + _v2.X) * (-_v1.Z + _v3.Z));
}
if (s1 >= 0 && s1 <= 1 && s2 >= 0 && s2 <= 1 && s3 >= 0 && s3 <= 1)
return true;
}
return false;
}
/// <summary>
/// Calculates the angle between two rays sharing the current vector
/// </summary>
/// <param name="v">current instance</param>
/// <param name="v1">vector on first ray</param>
/// <param name="v2">vector on second ray</param>
/// <returns>angle in radians</returns>
public static double AngleTo(this NUIVector v, NUIVector v1, NUIVector v2)
{
NUIVector diff1 = v1.Minus(v);
NUIVector diff2 = v2.Minus(v);
return Math.Acos(diff1.DotProduct(diff2) / diff1.Length() / diff2.Length());
}
/// <summary>
/// Calculates the distance of a vector to another one in 3D space
/// </summary>
/// <param name="v1">current instance</param>
/// <param name="v2">Second vector</param>
/// <returns>Distance</returns>
public static double DistanceTo(this NUIVector v1, NUIVector v2)
{
return v2.Minus(v1).Length();
}
