/// <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(); }