Rodrigues’ rotation formula

Rotate a point about some axis by some angle.

Rodrigues’ rotation formula
You are encouraged to solve this task according to the task description, using any language you may know.

Task

Described here: https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula

ALGOL 68

Translation of: JavaScript

<lang algol68>BEGIN # Rodrigues' Rotation Formula #

   MODE VECTOR = [ 3 ]REAL;
   MODE MATRIX = [ 3 ]VECTOR;
   PROC norm = ( VECTOR v )REAL: sqrt( ( v[1] * v[1] ) + ( v[2] * v[2] ) + ( v[3] * v[3] ) );
   PROC normalize = ( VECTOR v )VECTOR:
        BEGIN
           REAL length = norm( v );
           ( v[1] / length, v[2] / length, v[3] / length )
        END # normalize # ;
   PROC dot product = ( VECTOR v1, v2 )REAL: ( v1[1] * v2[1] ) + ( v1[2] * v2[2] ) + ( v1[3] * v2[3] );
   PROC cross product = ( VECTOR v1, v2 )VECTOR: ( ( v1[2] * v2[3] ) - ( v1[3] * v2[2] )
                                                 , ( v1[3] * v2[1] ) - ( v1[1] * v2[3] )
                                                 , ( v1[1] * v2[2] ) - ( v1[2] * v2[1] )
                                                 );
   PROC get angle = ( VECTOR v1, v2 )REAL: acos( dot product( v1, v2 ) / ( norm( v1 ) * norm( v2 ) ) );
   PROC matrix multiply = ( MATRIX m, VECTOR v )VECTOR: ( dot product( m[1], v )
                                                        , dot product( m[2], v )
                                                        , dot product( m[3], v )
                                                        );
   PROC a rotate = ( VECTOR p, v, REAL a )VECTOR:
        BEGIN
           REAL ca = cos( a ), sa = sin( a ), t = 1 - ca, x = v[1],  y = v[2], z = v[3];
           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 ) )
                      );
           matrix multiply( r, p )
        END # a rotate # ;
   VECTOR v1  = ( 5, -6, 4 );
   VECTOR v2  = ( 8, 5, -30 );
   REAL   a   = get angle( v1, v2 );
   VECTOR cp  = cross product( v1, v2 );
   VECTOR ncp = normalize( cp );
   VECTOR np  = a rotate( v1, ncp, a );
   print( ( fixed( np[ 1 ], -10, 6 )
          , fixed( np[ 2 ], -10, 6 )
          , fixed( np[ 3 ], -10, 6 )
          , newline
          )
        )

END</lang>

Output:

  2.232221  1.395138 -8.370829

JavaScript

<lang javascript>

function norm(v) {

   return Math.sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);

} function normalize(v) {

   var length = norm(v);
   return [v[0]/length, v[1]/length, v[2]/length];

} function dotProduct(v1, v2) {

   return v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2];

} function crossProduct(v1, v2) {

   return [v1[1]*v2[2] - v1[2]*v2[1], v1[2]*v2[0] - v1[0]*v2[2], v1[0]*v2[1] - v1[1]*v2[0]];

} function getAngle(v1, v2) {

   return Math.acos(dotProduct(v1, v2) / (norm(v1)*norm(v2)));

} function matrixMultiply(matrix, v) {

   return [dotProduct(matrix[0], v), dotProduct(matrix[1], v), dotProduct(matrix[2], v)];

} function aRotate(p, v, a) {

   var ca = Math.cos(a), sa = Math.sin(a), t=1-ca, x=v[0], y=v[1], z=v[2];
   var 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);

}

var v1 = [5,-6,4]; var v2 = [8,5,-30]; var a = getAngle(v1, v2); var cp = crossProduct(v1, v2); var ncp = normalize(cp); var np = aRotate(v1, ncp, a); console.log(np);

</lang>

Nim

Translation of: Wren

Only changed most function names. <lang Nim>import math

type

 Vector = tuple[x, y, z: float]
 Matrix = array[3, Vector]

func norm(v: Vector): float =

 sqrt(v.x * v.x + v.y * v.y + v.z * v.z)

func normalized(v: Vector): Vector =

 let length = v.norm()
 result = (v.x / length, v.y / length, v.z / length)

func scalarProduct(v1, v2: Vector): float =

 v1.x * v2.x + v1.y * v2.y + v1.z * v2.z

func vectorProduct(v1, v2: Vector): Vector =

 (v1.y * v2.z - v1.z * v2.y, v1.z * v2.x - v1.x * v2.z, v1.x * v2.y - v1.y * v2.x)

func angle(v1, v2: Vector): float =

 arccos(scalarProduct(v1, v2) / (norm(v1) * norm(v2)))

func `*`(m: Matrix; v: Vector): Vector =

 (scalarProduct(m[0], v), scalarProduct(m[1], v), scalarProduct(m[2], v))

func rotate(p, v: Vector; a: float): Vector =

 let ca = cos(a)
 let sa = sin(a)
 let t = 1 - ca
 let r = [(ca + v.x * v.x * t, v.x * v.y * t - v.z * sa, v.x * v.z * t + v.y * sa),
          (v.x * v.y * t + v.z * sa, ca + v.y * v.y * t, v.y * v.z * t - v.x * sa),
          (v.z * v.x * t - v.y * sa, v.z * v.y * t + v.x * sa, ca + v.z * v.z * t)]
 result = r * p

let

 v1 = (5.0, -6.0, 4.0)
 v2 = (8.0, 5.0, -30.0)
 a = angle(v1, v2)
 vp = vectorProduct(v1, v2)
 nvp = normalized(vp)
 np = v1.rotate(nvp, a)

echo np</lang>

Output:

(x: 2.232221073308228, y: 1.395138170817643, z: -8.370829024905852)

Perl

<lang perl>

1. !perl -w

use strict; use Math::Trig; # acos use JSON; use constant PI => 3.14159265358979;

1. Rodrigues' formula for vector rotation - see https://stackoverflow.com/questions/42358356/rodrigues-formula-for-vector-rotation

sub norm {

 my($v)=@_;
 return ($v->[0]*$v->[0] + $v->[1]*$v->[1] + $v->[2]*$v->[2]) ** 0.5;

} sub normalize {

 my($v)=@_;
 my $length = &norm($v);
 return [$v->[0]/$length, $v->[1]/$length, $v->[2]/$length];

} sub dotProduct {

 my($v1, $v2)=@_;
 return $v1->[0]*$v2->[0] + $v1->[1]*$v2->[1] + $v1->[2]*$v2->[2];

} sub crossProduct {

 my($v1, $v2)=@_;
 return [$v1->[1]*$v2->[2] - $v1->[2]*$v2->[1], $v1->[2]*$v2->[0] - $v1->[0]*$v2->[2], $v1->[0]*$v2->[1] - $v1->[1]*$v2->[0]];

} sub getAngle {

 my($v1, $v2)=@_;
 return acos(&dotProduct($v1, $v2) / (&norm($v1)*&norm($v2)))*180/PI;  # remove *180/PI to go back to radians

} sub matrixMultiply {

 my($matrix, $v)=@_;
 return [&dotProduct($matrix->[0], $v), &dotProduct($matrix->[1], $v), &dotProduct($matrix->[2], $v)];

} sub aRotate {

 my($p, $v, $a)=@_;    # point-to-rotate, vector-to-rotate-about, angle(degrees)
 my $ca = cos($a/180*PI);      # remove /180*PI to go back to radians
 my $sa = sin($a/180*PI);
 my $t=1-$ca;
 my($x,$y,$z)=($v->[0], $v->[1], $v->[2]);
 my $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);

}

my $v1 = [5,-6,4]; my $v2 = [8,5,-30]; my $a = &getAngle($v1, $v2); my $cp = &crossProduct($v1, $v2); my $ncp = &normalize($cp); my $np = &aRotate($v1, $ncp, $a);

my $json=JSON->new->canonical;

print $json->encode($np) . "\n"; # => [ 2.23222107330823, 1.39513817081764, -8.37082902490585 ] = ok. </lang>

Wren

Translation of: JavaScript

<lang ecmascript>var norm = Fn.new { |v| (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]).sqrt }

var normalize = Fn.new { |v|

   var length = norm.call(v)
   return [v[0]/length, v[1]/length, v[2]/length]

}

var dotProduct = Fn.new { |v1, v2| v1[0]*v2[0] + v1[1]*v2[1] + v1[2]*v2[2] }

var crossProduct = Fn.new { |v1, v2|

   return [v1[1]*v2[2] - v1[2]*v2[1], v1[2]*v2[0] - v1[0]*v2[2], v1[0]*v2[1] - v1[1]*v2[0]]

}

var getAngle = Fn.new { |v1, v2| (dotProduct.call(v1, v2) / (norm.call(v1) * norm.call(v2))).acos }

var matrixMultiply = Fn.new { |matrix, v|

   return [dotProduct.call(matrix[0], v), dotProduct.call(matrix[1], v), dotProduct.call(matrix[2], v)]

}

var aRotate = Fn.new { |p, v, a|

   var ca = a.cos
   var sa = a.sin
   var t = 1 - ca
   var x = v[0]
   var y = v[1]
   var z = v[2]
   var 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.call(r, p)

}

var v1 = [5, -6, 4] var v2 = [8, 5,-30] var a = getAngle.call(v1, v2) var cp = crossProduct.call(v1, v2) var ncp = normalize.call(cp) var np = aRotate.call(v1, ncp, a) System.print(np)</lang>

Output:

[2.2322210733082, 1.3951381708176, -8.3708290249059]