Task: Rodrigues’ rotation formula




Go: package main

import (
    "fmt"
    "math"
)

type vector [3]float64
type matrix [3]vector

func norm(v vector) float64 {
    return math.Sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2])
}

func normalize(v vector) vector {
    length := norm(v)
    return vector{v[0] / length, v[1] / length, v[2] / length}
}

func dotProduct(v1, v2 vector) float64 {
    return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2]
}

func crossProduct(v1, v2 vector) vector {
    return vector{v1[1]*v2[2] - v1[2]*v2[1], v1[2]*v2[0] - v1[0]*v2[2], v1[0]*v2[1] - v1[1]*v2[0]}
}

func getAngle(v1, v2 vector) float64 {
    return math.Acos(dotProduct(v1, v2) / (norm(v1) * norm(v2)))
}

func matrixMultiply(m matrix, v vector) vector {
    return vector{dotProduct(m[0], v), dotProduct(m[1], v), dotProduct(m[2], v)}
}

func aRotate(p, v vector, a float64) vector {
    ca, sa := math.Cos(a), math.Sin(a)
    t := 1 - ca
    x, y, z := v[0], v[1], v[2]
    r := matrix{
        {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)
}

func main() {
    v1 := vector{5, -6, 4}
    v2 := vector{8, 5, -30}
    a := getAngle(v1, v2)
    cp := crossProduct(v1, v2)
    ncp := normalize(cp)
    np := aRotate(v1, ncp, a)
    fmt.Println(np)
}








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;