Task: Rodrigues’ rotation formula





Java: 

class Vector{
  private double x, y, z;

  public Vector(double x1,double y1,double z1){
    x = x1;
    y = y1;
    z = z1;
  }
  
  void printVector(int x,int y){
    text("( " + this.x + " )  \u00ee + ( " + this.y + " ) + \u0135 ( " + this.z + ") \u006b\u0302",x,y);
  }

  public double norm() {
    return Math.sqrt(this.x*this.x + this.y*this.y + this.z*this.z);
  }
  
  public Vector normalize(){
    double length = this.norm();
    return new Vector(this.x / length, this.y / length, this.z / length);
  }
  
  public double dotProduct(Vector v2) {
    return this.x*v2.x + this.y*v2.y + this.z*v2.z;
  }
  
  public Vector crossProduct(Vector v2) {
    return new Vector(this.y*v2.z - this.z*v2.y, this.z*v2.x - this.x*v2.z, this.x*v2.y - this.y*v2.x);
  }
  
  public double getAngle(Vector v2) {
    return Math.acos(this.dotProduct(v2) / (this.norm()*v2.norm()));
  }
  
  public Vector aRotate(Vector v, double a) {
    double ca = Math.cos(a), sa = Math.sin(a);
    double t = 1.0 - ca;
    double x = v.x, y = v.y, z = v.z;
    Vector[] r = {
        new Vector(ca + x*x*t, x*y*t - z*sa, x*z*t + y*sa),
        new Vector(x*y*t + z*sa, ca + y*y*t, y*z*t - x*sa),
        new Vector(z*x*t - y*sa, z*y*t + x*sa, ca + z*z*t)
    };
    return new Vector(this.dotProduct(r[0]), this.dotProduct(r[1]), this.dotProduct(r[2]));
  }
}

void setup(){
  Vector v1 = new Vector(5d, -6d, 4d),v2 = new Vector(8d, 5d, -30d);
  double a = v1.getAngle(v2);
  Vector cp = v1.crossProduct(v2);
  Vector normCP = cp.normalize();
  Vector np = v1.aRotate(normCP,a);
  
  size(1200,600);
  fill(#000000);
  textSize(30);
  
  text("v1 = ",10,100);
  v1.printVector(60,100);
  text("v2 = ",10,150);
  v2.printVector(60,150);
  text("rV = ",10,200);
  np.printVector(60,200);
}







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;