I'm working on modernizing Rosetta Code's infrastructure. Starting with communications. Please accept this time-limited open invite to RC's Slack.. --Michael Mol (talk) 20:59, 30 May 2020 (UTC)

Rodrigues’ rotation formula

From Rosetta Code
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 using Rodrigues' rotation formula.


Reference


ALGOL 68[edit]

Translation of: JavaScript
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
Output:
(   2.232221,   1.395138,  -8.370829 )

C[edit]

Translation of: JavaScript
#include <stdio.h>
#include <math.h>
 
typedef struct {
double x, y, z;
} vector;
 
typedef struct {
vector i, j, k;
} matrix;
 
double norm(vector v) {
return sqrt(v.x*v.x + v.y*v.y + v.z*v.z);
}
 
vector normalize(vector v){
double length = norm(v);
vector n = {v.x / length, v.y / length, v.z / length};
return n;
}
 
double dotProduct(vector v1, vector v2) {
return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
}
 
vector crossProduct(vector v1, vector v2) {
vector cp = {v1.y*v2.z - v1.z*v2.y, v1.z*v2.x - v1.x*v2.z, v1.x*v2.y - v1.y*v2.x};
return cp;
}
 
double getAngle(vector v1, vector v2) {
return acos(dotProduct(v1, v2) / (norm(v1)*norm(v2)));
}
 
vector matrixMultiply(matrix m ,vector v) {
vector mm = {dotProduct(m.i, v), dotProduct(m.j, v), dotProduct(m.k, v)};
return mm;
}
 
vector aRotate(vector p, vector v, double a) {
double ca = cos(a), sa = sin(a);
double t = 1.0 - ca;
double x = v.x, y = v.y, z = v.z;
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}
};
return matrixMultiply(r, p);
}
 
int main() {
vector v1 = {5, -6, 4}, v2 = {8, 5, -30};
double a = getAngle(v1, v2);
vector cp = crossProduct(v1, v2);
vector ncp = normalize(cp);
vector np = aRotate(v1, ncp, a);
printf("[%.13f, %.13f, %.13f]\n", np.x, np.y, np.z);
return 0;
}
Output:
[2.2322210733082, 1.3951381708176, -8.3708290249059]

Factor[edit]

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
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 .
Output:
{ 2.232221073308229 1.395138170817642 -8.370829024905852 }

Go[edit]

Translation of: JavaScript
package main
 
import (
"fmt"
"math"
)
 
type vector [3]float64
type matrix [3]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)
}
Output:
[2.2322210733082275 1.3951381708176436 -8.370829024905852]

JavaScript[edit]

JavaScript: ES5[edit]

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

JavaScript: ES6[edit]

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

(() => {
"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)
];
};
 
 
const dotProduct = xs =>
compose(
sum,
zipWith(a => b => a * b)(xs)
);
 
 
const matrixMultiply = matrix =>
compose(
flip(map)(matrix),
dotProduct
);
 
 
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);
 
 
// map :: (a -> b) -> [a] -> [b]
const map = f =>
// The list obtained by applying f
// to each element of xs.
// (The image of xs under f).
xs => [...xs].map(f);
 
 
// 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
);
})();
Output:
[
  2.2322210733082275,
  1.3951381708176431,
  -8.370829024905852
]

jq[edit]

Adapted from Wren

Works with: jq

Works with gojq, the Go implementation of jq

In the comments, the term "vector" is used to mean a (JSON) array of numbers. Some of the functions have been generalized to work with vectors of arbitrary length.

 
# v1 and v2 should be vectors of the same length.
def dotProduct(v1; v2): [v1, v2] | transpose | map(.[0] * .[1]) | add;
 
# Input: a vector
def norm: dotProduct(.; .) | sqrt;
 
# Input: a vector
def normalize: norm as $n | map(./$n);
 
# v1 and v2 should be 3-vectors
def crossProduct(v1; v2):
[v1[1]*v2[2] - v1[2]*v2[1], v1[2]*v2[0] - v1[0]*v2[2], v1[0]*v2[1] - v1[1]*v2[0]];
 
# v1 and v2 should be of equal length.
def getAngle(v1; v2):
(dotProduct(v1; v2) / ((v1|norm) * (v2|norm)))|acos ;
 
# Input: a matrix (i.e. an array of same-length vectors)
# $v should be the same length as the vectors in the matrix
def matrixMultiply($v):
map(dotProduct(.; $v)) ;
 
# $p - the point vector
# $v - the axis
# $a - the angle in radians
def aRotate($p; $v; $a):
{ca: ($a|cos),
sa: ($a|sin)}
| .t = (1 - .ca)
| .x = $v[0]
| .y = $v[1]
| .z = $v[2]
| [
[.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]
]
| matrixMultiply($p) ;
 
def example:
[5, -6, 4] as $v1
| [8, 5,-30] as $v2
| getAngle($v1; $v2) as $a
| (crossProduct($v1; $v2) | normalize) as $ncp
| aRotate($v1; $ncp; $a)
;
 
example
Output:
[2.2322210733082275,1.3951381708176436,-8.370829024905852]


Julia[edit]

Translation of: Perl
using LinearAlgebra # use builtin library for normalize, cross, dot
using JSON3
 
getangleradians(v1, v2) = acos(dot(v1, v2) / (norm(v1) * norm(v2)))
 
function rodrotate(pointvector, rotationvector, radians)
ca, sa = cos(radians), sin(radians)
t = 1 - ca
x, y, z = rotationvector
return [[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]'] * pointvector
end
 
v1 = [5, -6, 4]
v2 = [8, 5, -30]
a = getangleradians(v1, v2)
cp = cross(v1, v2)
ncp = normalize(cp)
np = rodrotate(v1, ncp, a)
JSON3.write(np) # "[2.2322210733082284,1.3951381708176411,-8.370829024905854]"
 

Nim[edit]

Translation of: Wren

Only changed most function names.

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
Output:
(x: 2.232221073308228, y: 1.395138170817643, z: -8.370829024905852)

Perl[edit]

Task-specific[edit]

#!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";
Output:
[2.23222107330823,1.39513817081764,-8.37082902490585]

Generalized[edit]

use strict;
use warnings;
use feature <say signatures>;
no warnings 'experimental::signatures';
 
use Math::Trig;
use List::Util 'sum';
use constant PI => 2 * atan2(1, 0);
 
sub norm ($v) { sqrt sum map { $_*$_ } @$v }
sub normalize ($v) { [ map { $_ / norm $v } @$v ] }
sub dotProduct ($v1, $v2) { sum map { $v1->[$_] * $v2->[$_] } 0..$#$v1 }
sub getAngle ($v1, $v2) { 180/PI * acos dotProduct($v1, $v2) / (norm($v1)*norm($v2)) }
sub mvMultiply ($m, $v) { [ map { dotProduct($_, $v) } @$m ] }
sub crossProduct ($v1, $v2) {
[$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 aRotate ( $p, $v, $a ) {
my $ca = cos $a/180*PI;
my $sa = sin $a/180*PI;
my $t = 1 - $ca;
my($x,$y,$z) = @$v;
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]
];
mvMultiply($r, $p)
}
 
my($v1,$v2) = ([5, -6, 4], [8, 5, -30]);
say join ' ', @{aRotate $v1, normalize(crossProduct $v1, $v2), getAngle $v1, $v2};
Output:
2.23222107330823 1.39513817081764 -8.37082902490585

Raku[edit]

sub infix:<> { [+] @^a »×« @^b }
sub norm (@v) { sqrt @v@v }
sub normalize (@v) { @v X/ @v.&norm }
sub getAngle (@v1,@v2) { 180/π × acos (@v1@v2) / (@v1.&norm × @v2.&norm) }
 
sub crossProduct ( @v1, @v2 ) {
my \a = <1 2 0>; my \b = <2 0 1>;
(@v1[a] »×« @v2[b]) »-« (@v1[b] »×« @v2[a])
}
 
sub aRotate ( @p, @v, $a ) {
my \ca = cos $a/180×π;
my \sa = sin $a/180×π;
my \t = 1 - ca;
my (\x,\y,\z) = @v;
map { @p$_ },
[ 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],
[x×t - y×sa, z×y×t + x×sa, ca + z×z×t]
}
 
my @v1 = [5,-6, 4];
my @v2 = [8, 5,-30];
say join ' ', aRotate @v1, normalize(crossProduct @v1, @v2), getAngle @v1, @v2;
Output:
2.232221073308229 1.3951381708176411 -8.370829024905852

Alternately, differing mostly in style:

sub infix:<> { sum @^v1 Z× @^v2 } # dot product
 
sub infix:<> (@v1, @v2) { # cross product
my \a = <1 2 0>; my \b = <2 0 1>;
@v1[a] »×« @v2[b] »-« @v1[b] »×« @v2[a]
}
 
sub norm (*@v) { sqrt @v@v }
 
sub normal (*@v) { @v X/ @v.&norm }
 
sub angle-between (@v1, @v2) { acos( (@v1@v2) / (@v1.&norm × @v2.&norm) ) }
 
sub infix:<> is equiv(&infix:<×>) { $^a × $^b } # invisible times
 
sub postfix:<°> (\d) { d × τ / 360 } # degrees to radians
 
sub rodrigues-rotate( @point, @axis,) {
my ( \cos𝜃, \sin𝜃 ) = cis().reals;
my ( \𝑥, \𝑦, \𝑧 ) = @axis;
my \𝑡 = 1 - cos𝜃;
 
map @point*, [
[ 𝑥²⁢𝑡 + cos𝜃, 𝑦⁢𝑥⁢𝑡 - 𝑧⁢sin𝜃, 𝑧⁢𝑥⁢𝑡 + 𝑦⁢sin𝜃 ],
[ 𝑥⁢𝑦⁢𝑡 + 𝑧⁢sin𝜃, 𝑦²⁢𝑡 + cos𝜃, 𝑧⁢𝑦⁢𝑡 - 𝑥⁢sin𝜃 ],
[ 𝑥⁢𝑧⁢𝑡 - 𝑦⁢sin𝜃, 𝑦⁢𝑧⁢𝑡 + 𝑥⁢sin𝜃, 𝑧²⁢𝑡 + cos𝜃 ]
]
}
 
sub point-vector (@point, @vector) {
rodrigues-rotate @point, normal(@point@vector), angle-between @point, @vector
}
 
put qq:to/TESTING/;
Task example - Point and composite axis / angle:
{ point-vector [5, -6, 4], [8, 5, -30] }
 
Perhaps more useful, (when calculating a range of motion for a robot appendage,
for example), feeding a point, axis of rotation and rotation angle separately;
since theoretically, the point vector and axis of rotation should be constant:
 
{
(0, 10, 20 ... 180).map( { # in degrees
sprintf "Rotated %3d°: %.13f, %.13f, %.13f", $_,
rodrigues-rotate [5, -6, 4], ([5, -6, 4][8, 5, -30]).&normal, .°
}).join: "\n"
}
TESTING
Output:
Task example - Point and composite axis / angle:
2.232221073308228 1.3951381708176427 -8.370829024905852

Perhaps more useful, (when calculating a range of motion for a robot appendage,
for example), feeding a point, axis of rotation and rotation angle directly;
since theoretically, the point vector and axis of rotation should be constant:

Rotated   0°: 5.0000000000000, -6.0000000000000, 4.0000000000000
Rotated  10°: 5.7240554466526, -6.0975296976939, 2.6561853906284
Rotated  20°: 6.2741883650704, -6.0097890410223, 1.2316639322573
Rotated  30°: 6.6336832449081, -5.7394439854392, -0.2302810114435
Rotated  40°: 6.7916170161550, -5.2947088286573, -1.6852289831393
Rotated  50°: 6.7431909410900, -4.6890966233686, -3.0889721249495
Rotated  60°: 6.4898764214992, -3.9410085899762, -4.3988584118384
Rotated  70°: 6.0393702908772, -3.0731750048240, -5.5750876118134
Rotated  80°: 5.4053609500356, -2.1119645522518, -6.5819205958338
Rotated  90°: 4.6071124519719, -1.0865831254651, -7.3887652531624
Rotated 100°: 3.6688791733663, -0.0281864202486, -7.9711060171693
Rotated 110°: 2.6191688576205, 1.0310667150840, -8.3112487584187
Rotated 120°: 1.4898764214993, 2.0589914100238, -8.3988584118384
Rotated 130°: 0.3153148442246, 3.0243546928699, -8.2312730024418
Rotated 140°: -0.8688274150348, 3.8978244887705, -7.8135845280911
Rotated 150°: -2.0265707929363, 4.6528608599741, -7.1584842417190
Rotated 160°: -3.1227378427887, 5.2665224084086, -6.2858770340300
Rotated 170°: -4.1240220834695, 5.7201633384526, -5.2222766334692
Rotated 180°: -5.0000000000000, 6.0000000000000, -4.0000000000000

Wren[edit]

Translation of: JavaScript
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)
Output:
[2.2322210733082, 1.3951381708176, -8.3708290249059]