Aug 17, 2014

[C/C++] How to calculate angle between three points in 3D

// This program calculates angle between three points in 3D.
//
// 3 points = A(Ax, Ay, Az), B(Bx, By, Bz), C(Cx, Cy, Cz)
// To calculate angle ABC,
//      vector BA = (Ax-Bx, Ay-By, Az-Bz)
//      vector BC = (Cx-Bx, Cy-By, Cz-Bz)
// The dot product of vector BA and BC is a function of the cosine of the angle ABC, i.e., it's scaled by the product of their magnitude.
// So,
//      (1) normalize vector BA and BC with the product of their magnitude
//      (2) calculate the dot product
//      (3) recover the angle
 
 
#include <stdio.h>
#include <math.h>
 
 
void main(void)
{
    float Ax, Ay, Az, Bx, By, Bz, Cx, Cy, Cz;
 
    printf("\nThis program calculates angle of three points (ABC).\n");
    printf("\nEnter 'x,y,z' of point A : ");
    scanf("%f,%f,%f", &Ax, &Ay, &Az);
    printf("Enter 'x,y,z' of point B : ");
    scanf("%f,%f,%f", &Bx, &By, &Bz);
    printf("Enter 'x,y,z' of point C : ");
    scanf("%f,%f,%f", &Cx, &Cy, &Cz);
 
    float vector_BA_x = Ax-Bx;
    float vector_BA_y = Ay-By;
    float vector_BA_z = Az-Bz;
    float vector_BC_x = Cx-Bx;
    float vector_BC_y = Cy-By;
    float vector_BC_z = Cz-Bz;
 
    float magnitude_BA = sqrt(vector_BA_x*vector_BA_x + vector_BA_y*vector_BA_y + vector_BA_z*vector_BA_z);
    float magnitude_BC = sqrt(vector_BC_x*vector_BC_x + vector_BC_y*vector_BC_y + vector_BC_z*vector_BC_z);
 
    // (1) normalize vector BA and BC with the product of their magnitude
    float normalized_BA_x = vector_BA_x / magnitude_BA;
    float normalized_BA_y = vector_BA_y / magnitude_BA;
    float normalized_BA_z = vector_BA_z / magnitude_BA;
    float normalized_BC_x = vector_BC_x / magnitude_BC;
    float normalized_BC_y = vector_BC_y / magnitude_BC;
    float normalized_BC_z = vector_BC_z / magnitude_BC;
 
    // (2) calculate the dot product
    float dot = normalized_BA_x * normalized_BC_x  +  normalized_BA_y * normalized_BC_y  +  normalized_BA_z * normalized_BC_z;
 
    // (3) recover the angle
    float angle = acos(dot);
    float angle_degree = angle * 180 / 3.14159265359;
 
 
    printf("\n\tAngle ABC = %.2f degrees\n", angle_degree);
 
}
 

No comments:

Post a Comment