/// <summary>
/// Returns the angle between 0 and 2*pi between the two vectors given.
/// </summary>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <returns></returns>
public static Radian Angle(VectorF2D v1, VectorF2D v2)
{
double size_v1 = v1.Size;
double size_v2 = v2.Size;
double dot = VectorF2D.Dot(v1, v2);
double cross = VectorF2D.Cross(v1, v2);
// filter out the vectors that are parallel.
if (v1[0] == v2[0] &&
v1[1] == v2[1])
{
return(0);
}
else if (v1[0] == v2[0] &&
v1[1] == -v2[1])
{
return(System.Math.PI / 2.0f);
}
else if (v1[0] == -v2[0] &&
v1[1] == v2[1])
{
return(-System.Math.PI / 2.0f);
}
else if (v1[0] == -v2[0] &&
v1[1] == -v2[1])
{
return(System.Math.PI);
}
// split per quadrant.
double angle;
if (dot > 0)
{ // dot > 0
if (cross > 0)
{ // dot > 0 and cross > 0
// Quadrant 1
angle = (double)System.Math.Asin(cross / (size_v1 * size_v2));
if (angle < System.Math.PI / 4f)
{ // use cosine.
angle = (double)System.Math.Acos(dot / (size_v1 * size_v2));
}
// angle is ok here for quadrant 1.
}
else
{ // dot > 0 and cross <= 0
// Quadrant 4
angle = (double)(System.Math.PI * 2.0f) + (double)System.Math.Asin(cross / (size_v1 * size_v2));
if (angle > (double)(System.Math.PI * 2.0f) - System.Math.PI / 4f)
{ // use cosine.
angle = (double)(System.Math.PI * 2.0f) - (double)System.Math.Acos(dot / (size_v1 * size_v2));
}
// angle is ok here for quadrant 1.
}
}
else
{ // dot <= 0
if (cross > 0)
{ // dot > 0 and cross > 0
// Quadrant 2
angle = (double)System.Math.PI - (double)System.Math.Asin(cross / (size_v1 * size_v2));
if (angle > System.Math.PI / 2f + System.Math.PI / 4f)
{ // use cosine.
angle = (double)System.Math.Acos(dot / (size_v1 * size_v2));
}
// angle is ok here for quadrant 2.
}
else
{ // dot > 0 and cross <= 0
// Quadrant 3
angle = -(-(double)System.Math.PI + (double)System.Math.Asin(cross / (size_v1 * size_v2)));
if (angle < System.Math.PI + System.Math.PI / 4f)
{ // use cosine.
angle = (double)(System.Math.PI * 2.0f) - (double)System.Math.Acos(dot / (size_v1 * size_v2));
}
// angle is ok here for quadrant 3.
}
}
return(angle);
}
public void Vector2DTest()
{
// create the test cases.
VectorF2D a_b = new VectorF2D(1 , 1);
VectorF2D b_a = new VectorF2D(-1,-1);
VectorF2D a_c = new VectorF2D(-1, 1);
VectorF2D a_d = new VectorF2D(0, 1);
VectorF2D a_e = new VectorF2D(-1, 0);
VectorF2D a_f = new VectorF2D(0, 1);
VectorF2D a_g = new VectorF2D(0, -1);
// calculate the results
double sqrt_2 = (double)System.Math.Sqrt(2);
// check the sizes.
Assert.AreEqual(a_b.Size, sqrt_2, string.Format("Size should be {0}!", sqrt_2));
Assert.AreEqual(b_a.Size, sqrt_2, string.Format("Size should be {0}!", sqrt_2));
// check the equality.
Assert.IsTrue(a_b.Inverse == b_a, "The inverse of ab should be ba!");
// check the cross product.
Assert.AreEqual(VectorF2D.Cross(a_b, b_a), 0, "Cross product of two parallel vectors should be 0!");
Assert.AreEqual(VectorF2D.Cross(a_b, a_c), 2, string.Format("Cross product of two perpendicular vectors should be maximized; in this case {0}!", 2));
Assert.AreEqual(VectorF2D.Cross(b_a, a_b), 0, "Cross product of two parallel vectors should be 0!");
Assert.AreEqual(VectorF2D.Cross(a_c, a_b), -2, string.Format("Cross product of two perpendicular vectors should be maximized; in this case {0}!", -2));
// check the dot product.
Assert.AreEqual(VectorF2D.Dot(a_b, b_a), -2, string.Format("Cross product of two parallel vectors should be maximized (absolute value); in this case {0}!", -2));
Assert.AreEqual(VectorF2D.Dot(a_b, a_c), 0, string.Format("Cross product of two perpendicular vectors should be {0}!", 0));
Assert.AreEqual(VectorF2D.Dot(a_b, a_d), 1);
Assert.AreEqual(VectorF2D.Dot(a_b, a_e), -1);
Assert.AreEqual(VectorF2D.Dot(a_b, a_f), 1);
Assert.AreEqual(VectorF2D.Dot(a_b, a_g), -1);
Assert.AreEqual(VectorF2D.Dot(b_a, a_b), -2, string.Format("Cross product of two parallel vectors should be maximized; in this case {0}!", -2));
Assert.AreEqual(VectorF2D.Dot(a_c, a_b), 0, string.Format("Cross product of two perpendicular vectors should be {0}!", 0));
}
/// <summary>
/// Compares the two vectors just based on their direction.
/// </summary>
/// <param name="other"></param>
/// <returns></returns>
public bool CompareNormalized(VectorF2D other)
{ // be a sensitive as possible.
return(this.CompareNormalized(other, 0));
}
/// <summary>
/// Calculates the cross product.
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static double Cross(VectorF2D a, VectorF2D b)
{
return(a[0] * b[1] - a[1] * b[0]);
}
public bool CompareNormalized(VectorF2D other)
{
return(this.CompareNormalized(other, 0.0));
}
/// <summary>
/// Returns the angle between this vector and the given vector in the range 0-2pi.
/// </summary>
/// <param name="v"></param>
/// <returns></returns>
public Radian Angle(VectorF2D v)
{
return(VectorF2D.Angle(this, v));
}
/// <summary>
/// Compares the two vectors just based on their direction.
/// </summary>
/// <param name="other">The other vector to compare to.</param>
/// <param name="epsilon">The tolerance on the total difference between the normalized vectors.</param>
/// <returns></returns>
public bool CompareNormalized(VectorF2D other, double epsilon)
{
VectorF2D normalizedThis = this.Normalize();
VectorF2D normalizedOther = other.Normalize();
double difference = System.Math.Abs(normalizedThis[0] - normalizedOther[0]) +
System.Math.Abs(normalizedThis[1] - normalizedOther[1]);
return difference < epsilon;
}
public static Radian Angle(VectorF2D v1, VectorF2D v2)
{
double size1 = v1.Size;
double size2 = v2.Size;
double num1 = VectorF2D.Dot(v1, v2);
double num2 = VectorF2D.Cross(v1, v2);
if (v1[0] == v2[0] && v1[1] == v2[1])
{
return((Radian)0.0);
}
if (v1[0] == v2[0] && v1[1] == -v2[1])
{
return((Radian)(System.Math.PI / 2.0));
}
if (v1[0] == -v2[0] && v1[1] == v2[1])
{
return((Radian)(-1.0 * System.Math.PI / 2.0));
}
if (v1[0] == -v2[0] && v1[1] == -v2[1])
{
return((Radian)System.Math.PI);
}
double num3;
if (num1 > 0.0)
{
if (num2 > 0.0)
{
num3 = System.Math.Asin(num2 / (size1 * size2));
if (num3 < System.Math.PI / 4.0)
{
num3 = System.Math.Acos(num1 / (size1 * size2));
}
}
else
{
num3 = 2.0 * System.Math.PI + System.Math.Asin(num2 / (size1 * size2));
if (num3 > 7.0 * System.Math.PI / 4.0)
{
num3 = 2.0 * System.Math.PI - System.Math.Acos(num1 / (size1 * size2));
}
}
}
else if (num2 > 0.0)
{
num3 = System.Math.PI - System.Math.Asin(num2 / (size1 * size2));
if (num3 > 3.0 * System.Math.PI / 4.0)
{
num3 = System.Math.Acos(num1 / (size1 * size2));
}
}
else
{
num3 = -(System.Math.Asin(num2 / (size1 * size2)) - System.Math.PI);
if (num3 < 5.0 * System.Math.PI / 4.0)
{
num3 = 2.0 * System.Math.PI - System.Math.Acos(num1 / (size1 * size2));
}
}
return((Radian)num3);
}
/// <summary>
/// Compares the two vectors just based on their direction.
/// </summary>
/// <param name="other"></param>
/// <returns></returns>
public bool CompareNormalized(VectorF2D other)
{
// be a sensitive as possible.
return this.CompareNormalized(other, 0);
}
/// <summary>
/// Returns the angle between this vector and the given vector in the range 0-2pi.
/// </summary>
/// <param name="v"></param>
/// <returns></returns>
public Radian Angle(VectorF2D v)
{
return VectorF2D.Angle(this, v);
}
/// <summary>
/// Calcuates the dot-product of the two given vectors.
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static double Dot(VectorF2D a, VectorF2D b)
{
double dot = 0.0f;
for (int idx = 0; idx < 2; idx++)
{
dot = dot + a[idx] * b[idx];
}
return dot;
}
/// <summary>
/// Calculates the cross product.
/// </summary>
/// <param name="a"></param>
/// <param name="b"></param>
/// <returns></returns>
public static double Cross(VectorF2D a, VectorF2D b)
{
return a[0] * b[1] - a[1] * b[0];
}
/// <summary>
/// Returns the angle between 0 and 2*pi between the two vectors given.
/// </summary>
/// <param name="v1"></param>
/// <param name="v2"></param>
/// <returns></returns>
public static Radian Angle(VectorF2D v1, VectorF2D v2)
{
double size_v1 = v1.Size;
double size_v2 = v2.Size;
double dot = VectorF2D.Dot(v1, v2);
double cross = VectorF2D.Cross(v1, v2);
// filter out the vectors that are parallel.
if (v1[0] == v2[0]
&& v1[1] == v2[1])
{
return 0;
}
else if (v1[0] == v2[0]
&& v1[1] == -v2[1])
{
return System.Math.PI / 2.0f;
}
else if (v1[0] == -v2[0]
&& v1[1] == v2[1])
{
return -System.Math.PI / 2.0f;
}
else if (v1[0] == -v2[0]
&& v1[1] == -v2[1])
{
return System.Math.PI;
}
// split per quadrant.
double angle;
if (dot > 0)
{ // dot > 0
if (cross > 0)
{ // dot > 0 and cross > 0
// Quadrant 1
angle = (double)System.Math.Asin(cross / (size_v1 * size_v2));
if (angle < System.Math.PI / 4f)
{ // use cosine.
angle = (double)System.Math.Acos(dot / (size_v1 * size_v2));
}
// angle is ok here for quadrant 1.
}
else
{ // dot > 0 and cross <= 0
// Quadrant 4
angle = (double)(System.Math.PI * 2.0f) + (double)System.Math.Asin(cross / (size_v1 * size_v2));
if (angle > (double)(System.Math.PI * 2.0f) - System.Math.PI / 4f)
{ // use cosine.
angle = (double)(System.Math.PI * 2.0f) - (double)System.Math.Acos(dot / (size_v1 * size_v2));
}
// angle is ok here for quadrant 1.
}
}
else
{ // dot <= 0
if (cross > 0)
{ // dot > 0 and cross > 0
// Quadrant 2
angle = (double)System.Math.PI - (double)System.Math.Asin(cross / (size_v1 * size_v2));
if (angle > System.Math.PI / 2f + System.Math.PI / 4f)
{ // use cosine.
angle = (double)System.Math.Acos(dot / (size_v1 * size_v2));
}
// angle is ok here for quadrant 2.
}
else
{ // dot > 0 and cross <= 0
// Quadrant 3
angle = -(-(double)System.Math.PI + (double)System.Math.Asin(cross / (size_v1 * size_v2)));
if (angle < System.Math.PI + System.Math.PI / 4f)
{ // use cosine.
angle = (double)(System.Math.PI * 2.0f) - (double)System.Math.Acos(dot / (size_v1 * size_v2));
}
// angle is ok here for quadrant 3.
}
}
return angle;
}
