Rodrigues’ rotation formula: Difference between revisions

From Rosetta Code
Content added Content deleted
(→‎{{header|JavaScript}}: Added an ES6 variant.)
Line 160: Line 160:
===JavaScript: ES6===
===JavaScript: ES6===
(Returning a value directly and avoiding console.log,
(Returning a value directly and avoiding console.log,
which is often defined by browser libraries,
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)
but is not part of JavaScript's ECMAScript standards themselves,
and is not available to all JavaScript interpreters)


<lang JavaScript>(() => {
<lang JavaScript>(() => {

Revision as of 22:33, 29 September 2021

Task
Rodrigues’ rotation formula
You are encouraged to solve this task according to the task description, using any language you may know.
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 )

Go

Translation of: JavaSscript

<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=316330"