Rodrigues’ rotation formula

From Rosetta Code
Revision as of 00:33, 30 September 2021 by Chunes (talk | contribs) (→‎{{header|Factor}}: simplify)
Rodrigues’ rotation formula is a draft programming task. It is not yet considered ready to be promoted as a complete task, for reasons that should be found in its talk page.
Task

Rotate a point about some axis by some angle.

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 )

Factor

Note the following words already exist in Factor, which I have elected not to redefine:

Word Vocabulary Equivalent function in JavaScript (ES5) entry
normalize math.vectors normalize()
cross math.vectors crossProduct()
angle-between math.vectors getAngle()
mdotv math.matrices matrixMultiply()
Translation of: JavaScript
Works with: Factor version 0.99 2021-06-02

<lang factor>USING: grouping kernel math math.functions math.matrices math.vectors prettyprint sequences sequences.generalizations ;

a-rotate ( p v a -- seq )
   a cos a sin :> ( ca sa )
   ca 1 - v first3 :> ( t x y z )
   x x t * * ca + 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 * * +
   9 narray 3 group p mdotv ;

{ 5 -6 4 } { 8 5 -30 } dupd [ cross normalize ] [ angle-between ] 2bi a-rotate .</lang>

Output:
{ 2.232221073308229 1.395138170817642 -8.370829024905852 }

Go

Translation of: JavaScript

<lang go>package main

import ( 

   "fmt"
   "math"

)

type vector []float64 type matrix []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)

}</lang>

Output:
[2.2322210733082275 1.3951381708176436 -8.370829024905852]

JavaScript

JavaScript: ES5

<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>

JavaScript: ES6

(Returning a value directly and avoiding console.log, which is often defined by browser libraries,

but is not part of JavaScript's ECMAScript standards themselves, and is not available to all JavaScript interpreters)

<lang JavaScript>(() => { 

   "use strict";

   // --------------- RODRIGUES ROTATION ----------------

   const rodrigues = v1 =>
       v2 => aRotate(v1)(
           normalize(
               crossProduct(v1)(v2)
           )
       )(
           angle(v1)(v2)
       );

   // ---------------------- TEST -----------------------
   const main = () =>
       rodrigues([5, -6, 4])([8, 5, -30]);



   // ---------------- VECTOR FUNCTIONS -----------------
   const aRotate = p =>
       v => a => {
           const
               cosa = Math.cos(a),
               sina = Math.sin(a),
               t = 1 - cosa,
               [x, y, z] = v;

           return matrixMultiply([
               [
                   cosa + ((x ** 2) * t),
                   (x * y * t) - (z * sina),
                   (x * z * t) + (y * sina)
               ],
               [
                   (x * y * t) + (z * sina),
                   cosa + ((y ** 2) * t),
                   (y * z * t) - (x * sina)
               ],
               [
                   (z * x * t) - (y * sina),
                   (z * y * t) + (x * sina),
                   cosa + (z * z * t)
               ]
           ])(p);
       };



   const angle = v1 =>
       v2 => Math.acos(
           dotProduct(v1)(v2) / (
               norm(v1) * norm(v2)
           )
       );



   const crossProduct = xs =>
       // Cross product of two 3D vectors.
       ys => {
           const [x1, x2, x3] = xs;
           const [y1, y2, y3] = ys;

           return [
               (x2 * y3) - (x3 * y2),
               (x3 * y1) - (x1 * y3),
               (x1 * y2) - (x2 * y1)
           ];
       };



   // dotProduct :: Float -> Float -> Float
   const dotProduct = xs =>
       compose(sum, zipWith(a => b => a * b)(xs));



   const matrixMultiply = matrix =>
       v => matrix.map(flip(dotProduct)(v));



   const norm = v =>
       Math.sqrt(
           v.reduce((a, x) => a + (x ** 2), 0)
       );



   const normalize = v => {
       const len = norm(v);

       return v.map(x => x / len);
   };



   // --------------------- GENERIC ---------------------

   // compose (<<<) :: (b -> c) -> (a -> b) -> a -> c
   const compose = (...fs) =>
       // A function defined by the right-to-left
       // composition of all the functions in fs.
       fs.reduce(
           (f, g) => x => f(g(x)),
           x => x
       );



   // flip :: (a -> b -> c) -> b -> a -> c
   const flip = op =>
       // The binary function op with
       // its arguments reversed.
       x => y => op(y)(x);



   // sum :: [Num] -> Num
   const sum = xs =>
       // The numeric sum of all values in xs.
       xs.reduce((a, x) => a + x, 0);



   // zipWith :: (a -> b -> c) -> [a] -> [b] -> [c]
   const zipWith = f =>
       // A list constructed by zipping with a
       // custom function, rather than with the
       // default tuple constructor.
       xs => ys => xs.map(
           (x, i) => f(x)(ys[i])
       ).slice(
           0, Math.min(xs.length, ys.length)
       );



   return JSON.stringify(
       main(),
       null, 2
   );

})();</lang>

Output:
[
  2.2322210733082275,
  1.3951381708176431,
  -8.370829024905852
]

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]
Retrieved from "https://rosettacode.org/wiki/Rodrigues’_rotation_formula?oldid=316336"