Rodrigues’ rotation formula: Difference between revisions
(Added Go) |
(→{{header|JavaScript}}: Added an ES6 variant.) |
||
Line 120: | Line 120: | ||
=={{header|JavaScript}}== |
=={{header|JavaScript}}== |
||
===JavaScript: ES5=== |
|||
<lang javascript> |
|||
<lang javascript>function norm(v) { |
|||
function norm(v) { |
|||
return Math.sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]); |
return Math.sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]); |
||
} |
} |
||
Line 157: | Line 156: | ||
var ncp = normalize(cp); |
var ncp = normalize(cp); |
||
var np = aRotate(v1, ncp, a); |
var np = aRotate(v1, ncp, a); |
||
console.log(np); |
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( |
|||
</lang> |
|||
main(), |
|||
null, 2 |
|||
); |
|||
})();</lang> |
|||
{{Out}} |
|||
<pre>[ |
|||
2.2322210733082275, |
|||
1.3951381708176431, |
|||
-8.370829024905852 |
|||
]</pre> |
|||
=={{header|Nim}}== |
=={{header|Nim}}== |
Revision as of 22:32, 29 September 2021
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
<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
<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
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>
- !perl -w
use strict; use Math::Trig; # acos use JSON; use constant PI => 3.14159265358979;
- 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
<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]