Task: Rodrigues’ rotation formula

C: #include <stdio.h>
#include <math.h>

typedef struct {
    double x, y, z;
} vector;

typedef struct {
    vector i, j, k;
} matrix;

double norm(vector v) {
    return sqrt(v.x*v.x + v.y*v.y + v.z*v.z);
}

vector normalize(vector v){
    double length = norm(v);
    vector n = {v.x / length, v.y / length, v.z / length};
    return n;
}

double dotProduct(vector v1, vector v2) {
    return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
}

vector crossProduct(vector v1, vector v2) {
    vector cp = {v1.y*v2.z - v1.z*v2.y, v1.z*v2.x - v1.x*v2.z, v1.x*v2.y - v1.y*v2.x};
    return cp;
}

double getAngle(vector v1, vector v2) {
    return acos(dotProduct(v1, v2) / (norm(v1)*norm(v2)));
}

vector matrixMultiply(matrix m ,vector v) {
    vector mm = {dotProduct(m.i, v), dotProduct(m.j, v), dotProduct(m.k, v)};
    return mm;
}

vector aRotate(vector p, vector v, double a) {
    double ca = cos(a), sa = sin(a);
    double t = 1.0 - ca;
    double x = v.x, y = v.y, z = v.z;
    matrix r = {
        {ca + x*x*t, x*y*t - z*sa, x*z*t + y*sa},
        {x*y*t + z*sa, ca + y*y*t, y*z*t - x*sa},
        {z*x*t - y*sa, z*y*t + x*sa, ca + z*z*t}
    };
    return matrixMultiply(r, p);
}

int main() {
    vector v1 = {5, -6, 4}, v2 = {8, 5, -30};
    double a = getAngle(v1, v2);
    vector cp = crossProduct(v1, v2);
    vector ncp = normalize(cp);
    vector np = aRotate(v1, ncp, a);
    printf("[%.13f, %.13f, %.13f]\n", np.x, np.y, np.z);
    return 0;
}











Ada: with Ada.Text_Io;
use  Ada.Text_Io;
with Ada.Numerics.Elementary_Functions;
use  Ada.Numerics.Elementary_Functions;
procedure Rodrigues is
   type Vector is record
      X, Y, Z : Float;
   end record;
   function Image (V : in Vector) return String is
     ('[' & V.X'Image & ',' & V.Y'Image & ',' & V.Z'Image & ']');
   
   function "+" (V1, V2 : in Vector)           return Vector is ((V1.X + V2.X,
                                                                  V1.Y + V2.Y,
                                                                  V1.Z + V2.Z));
   function "*" (V : in Vector; A : in Float)  return Vector is ((V.X*A, V.Y*A, V.Z*A));
   function "/" (V : in Vector; A : in Float)  return Vector is (V*(1.0/A));
   function "*" (V1, V2 : in Vector)           return Float  is (
                                                                 (V1.X*V2.X + V1.Y*V2.Y + V1.Z*V2.Z));
   function Norm(V : in Vector)                return Float  is (Sqrt(V*V));
   function Normalize(V : in Vector)           return Vector is (V /Norm(V));
   function Cross_Product (V1, V2 : in Vector) return Vector is (
                                                                 (V1.Y*V2.Z - V1.Z*V2.Y,
                                                                  V1.Z*V2.X - V1.X*V2.Z,
                                                                  V1.X*V2.Y - V1.Y*V2.X));
   function Angle (V1, V2 : in Vector)         return Float  is (Arccos((V1*V2) / (Norm(V1)*Norm(V2))));
   
   function Rotate (V, Axis : in Vector;
                    Theta   : in Float) return Vector is
      K : constant Vector := Normalize(Axis);
   begin 
      return V*Cos(Theta) + Cross_Product(K,V)*Sin(Theta) + K*(K*V)*(1.0-Cos(Theta));
   end Rotate;
   
   
   Source : constant Vector := ( 0.0, 2.0, 1.0);
   Target : constant Vector := (-1.0, 2.0, 0.4);
begin
   Put_Line ("Vector     " & Image(Source));
   Put_Line ("rotated on " & Image(Target));
   Put_Line ("         = " & Image(Rotate(V     => Source,
                                          Axis  => Cross_Product(Source, Target),
                                          Theta => Angle(Source, Target))));
end Rodrigues;