Rosetta Code

Rodrigues’ rotation formula: Difference between revisions

Page Discussion

Rodrigues’ rotation formula (view source)

Revision as of 09:55, 3 February 2024

32,386 bytes added ,  3 months ago
m
→‎{{header|Wren}}: Changed to Wren S/H
m (julia example)
m (→‎{{header|Wren}}: Changed to Wren S/H)
 
(21 intermediate revisions by 12 users not shown)
Line 2:
 
;Task
Rotate a point about some axis by some angle.
 
DescribedRotate here:a httpspoint about some axis by some angle using [[wp://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula|Rodrigues' rotation formula]].
 
 
;Reference
* [[wp:Rodrigues%27_rotation_formula|Wikipedia: Rodrigues' rotation formula]]
 
 
 
=={{header|Ada}}==
<syntaxhighlight lang="ada">with Ada.Text_Io;
use Ada.Text_Io;
with Ada.Numerics.Elementary_Functions;
use Ada.Numerics.Elementary_Functions;
procedure Rodrigues is
type Vector is record
X, Y, Z : Float;
end record;
function Image (V : in Vector) return String is
('[' & V.X'Image & ',' & V.Y'Image & ',' & V.Z'Image & ']');
-- Basic operations
function "+" (V1, V2 : in Vector) return Vector is ((V1.X + V2.X,
V1.Y + V2.Y,
V1.Z + V2.Z));
function "*" (V : in Vector; A : in Float) return Vector is ((V.X*A, V.Y*A, V.Z*A));
function "/" (V : in Vector; A : in Float) return Vector is (V*(1.0/A));
function "*" (V1, V2 : in Vector) return Float is (-- dot-product
(V1.X*V2.X + V1.Y*V2.Y + V1.Z*V2.Z));
function Norm(V : in Vector) return Float is (Sqrt(V*V));
function Normalize(V : in Vector) return Vector is (V /Norm(V));
function Cross_Product (V1, V2 : in Vector) return Vector is (-- cross-product
(V1.Y*V2.Z - V1.Z*V2.Y,
V1.Z*V2.X - V1.X*V2.Z,
V1.X*V2.Y - V1.Y*V2.X));
function Angle (V1, V2 : in Vector) return Float is (Arccos((V1*V2) / (Norm(V1)*Norm(V2))));
-- Rodrigues' rotation formula
function Rotate (V, Axis : in Vector;
Theta : in Float) return Vector is
K : constant Vector := Normalize(Axis);
begin
return V*Cos(Theta) + Cross_Product(K,V)*Sin(Theta) + K*(K*V)*(1.0-Cos(Theta));
end Rotate;
--
-- Rotate vector Source on Target
Source : constant Vector := ( 0.0, 2.0, 1.0);
Target : constant Vector := (-1.0, 2.0, 0.4);
begin
Put_Line ("Vector " & Image(Source));
Put_Line ("rotated on " & Image(Target));
Put_Line (" = " & Image(Rotate(V => Source,
Axis => Cross_Product(Source, Target),
Theta => Angle(Source, Target))));
end Rodrigues;
</syntaxhighlight>
{{out}}
<pre>
Vector [ 0.00000E+00, 2.00000E+00, 1.00000E+00]
rotated on [-1.00000E+00, 2.00000E+00, 4.00000E-01]
= [-9.84374E-01, 1.96875E+00, 3.93750E-01]
</pre>
 
=={{header|ALGOL 68}}==
{{Trans|JavaScript}}
<langsyntaxhighlight lang="algol68">BEGIN # Rodrigues' Rotation Formula #
MODE VECTOR = [ 3 ]REAL;
MODE MATRIX = [ 3 ]VECTOR;
Line 49 ⟶ 105:
)
)
END</langsyntaxhighlight>
{{out}}
<pre>
( 2.232221, 1.395138, -8.370829 )
</pre>
 
=={{header|AutoHotkey}}==
{{Trans|JavaScript}}
<syntaxhighlight lang="autohotkey">v1 := [5,-6,4]
v2 := [8,5,-30]
a := getAngle(v1, v2)
cp := crossProduct(v1, v2)
ncp := normalize(cp)
np := aRotate(v1, ncp, a)
for i, v in np
result .= v ", "
MsgBox % result := "[" Trim(result, ", ") "]"
return
 
norm(v) {
return Sqrt(v[1]*v[1] + v[2]*v[2] + v[3]*v[3])
}
normalize(v) {
length := norm(v)
return [v[1]/length, v[2]/length, v[3]/length]
}
dotProduct(v1, v2) {
return v1[1]*v2[1] + v1[2]*v2[2] + v1[3]*v2[3]
}
crossProduct(v1, v2) {
return [v1[2]*v2[3] - v1[3]*v2[2], v1[3]*v2[1] - v1[1]*v2[3], v1[1]*v2[2] - v1[2]*v2[1]]
}
getAngle(v1, v2) {
return ACos(dotProduct(v1, v2) / (norm(v1)*norm(v2)))
}
matrixMultiply(matrix, v) {
return [dotProduct(matrix[1], v), dotProduct(matrix[2], v), dotProduct(matrix[3], v)]
}
aRotate(p, v, a) {
ca:=Cos(a), sa:=Sin(a), t:=1-ca, x:=v[1], y:=v[2], z:=v[3]
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)
}</syntaxhighlight>
{{out}}
<pre>[2.232221, 1.395138, -8.370829]</pre>
 
=={{header|C}}==
{{trans|JavaScript}}
<syntaxhighlight lang="c">#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;
}</syntaxhighlight>
 
{{out}}
<pre>
[2.2322210733082, 1.3951381708176, -8.3708290249059]
</pre>
 
Line 83 ⟶ 249:
{{trans|JavaScript}}
{{works with|Factor|0.99 2021-06-02}}
<langsyntaxhighlight lang="factor">USING: grouping kernel math math.functions math.matrices
math.vectors prettyprint sequences sequences.generalizations ;
 
Line 95 ⟶ 261:
 
{ 5 -6 4 } { 8 5 -30 }
dupd [ cross normalize ] [ angle-between ] 2bi a-rotate .</langsyntaxhighlight>
{{out}}
<pre>
{ 2.232221073308229 1.395138170817642 -8.370829024905852 }
</pre>
 
=={{header|FreeBASIC}}==
This example rotates the vector [-1, 2, -0.4] around the axis [-1, 2, 1] in increments of 18 degrees.
<syntaxhighlight lang="freebasic">#define PI 3.14159265358979323
 
type vector
'define a 3 dimensional vector data type
x as double
y as double
z as double
end type
 
operator + ( a as vector, b as vector) as vector
'vector addition
dim as vector r
r.x = a.x + b.x
r.y = a.y + b.y
r.z = a.z + b.z
return r
end operator
 
operator * ( a as vector, b as vector ) as double
'dot product
return a.x*b.x + a.y*b.y + a.z*b.z
end operator
 
operator * ( c as double, a as vector ) as vector
'multiplication of a scalar by a vector
dim as vector r
r.x = c*a.x
r.y = c*a.y
r.z = c*a.z
return r
end operator
 
function hat( a as vector ) as vector
'returns a unit vector in the direction of a
return (1.0/sqr(a*a))*a
end function
 
operator ^ ( a as vector, b as vector ) as vector
'cross product
dim as vector r
r.x = a.y*b.z - a.z*b.y
r.y = a.z*b.x - a.x*b.z
r.z = a.x*b.y - a.y*b.x
return r
end operator
 
function rodrigues( v as vector, byval k as vector, theta as double ) as vector
k = hat(k)
return cos(theta)*v + sin(theta)*(k^v) + (1-cos(theta))*(k*v)*k
end function
 
dim as vector k, v, r
dim as double theta
k.x = 0 : k.y = 2 : k.z = 1
v.x = -1 : v.y = 2 : v.z = 0.4
 
print "Theta rotated vector"
print "-----------------------------"
for theta = 0 to 2*PI step PI/10
r = rodrigues( v, k, theta )
print using "##.### [##.### ##.### ##.###]"; theta; r.x; r.y; r.z
next theta</syntaxhighlight>
{{out}}<pre>
Theta rotated vector
-----------------------------
0.000 [-1.000 2.000 0.400]
0.314 [-1.146 1.915 0.424]
0.628 [-1.269 1.818 0.495]
0.942 [-1.355 1.719 0.606]
1.257 [-1.397 1.629 0.745]
1.571 [-1.390 1.555 0.900]
1.885 [-1.335 1.505 1.055]
2.199 [-1.238 1.484 1.194]
2.513 [-1.107 1.494 1.305]
2.827 [-0.956 1.534 1.376]
3.142 [-0.800 1.600 1.400]
3.456 [-0.654 1.685 1.376]
3.770 [-0.531 1.782 1.305]
4.084 [-0.445 1.881 1.194]
4.398 [-0.403 1.971 1.055]
4.712 [-0.410 2.045 0.900]
5.027 [-0.465 2.095 0.745]
5.341 [-0.562 2.116 0.606]
5.655 [-0.693 2.106 0.495]
5.969 [-0.844 2.066 0.424]
6.283 [-1.000 2.000 0.400]
</pre>
 
=={{header|Go}}==
{{trans|JavaScript}}
<langsyntaxhighlight lang="go">package main
 
import (
Line 110 ⟶ 366:
)
 
type vector [3]float64
type matrix [3]vector
 
func norm(v vector) float64 {
Line 158 ⟶ 414:
np := aRotate(v1, ncp, a)
fmt.Println(np)
}</langsyntaxhighlight>
 
{{out}}
Line 167 ⟶ 423:
=={{header|JavaScript}}==
===JavaScript: ES5===
<langsyntaxhighlight lang="javascript">function norm(v) {
return Math.sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
}
Line 202 ⟶ 458:
var ncp = normalize(cp);
var np = aRotate(v1, ncp, a);
console.log(np); </langsyntaxhighlight>
 
===JavaScript: ES6===
Line 211 ⟶ 467:
and is not available to all JavaScript interpreters)
 
<langsyntaxhighlight JavaScriptlang="javascript">(() => {
"use strict";
 
Line 281 ⟶ 537:
 
 
// dotProduct :: [[Float]] -> [[Float]] -> [[Float]]
const dotProduct = xs =>
compose(sum, zipWith(a => b => a * b)(xs));
sum,
zipWith(a => b => a * b)(xs)
);
 
 
const matrixMultiply = matrix =>
compose(
v => matrix.map(flip(dotProduct)(v));
flip(map)(matrix),
dotProduct
);
 
 
Line 320 ⟶ 581:
// 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);
 
 
Line 344 ⟶ 613:
null, 2
);
})();</langsyntaxhighlight>
{{Out}}
<pre>[
Line 351 ⟶ 620:
-8.370829024905852
]</pre>
 
=={{header|jq}}==
'''Adapted from [[#Wren|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.
<syntaxhighlight lang="jq">
# 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</syntaxhighlight>
{{out}}
<pre>
[2.2322210733082275,1.3951381708176436,-8.370829024905852]
</pre>
 
 
=={{header|Julia}}==
{{trans|Perl}}
<langsyntaxhighlight lang="julia">using LinearAlgebra # use builtin library for normalize, cross, dot
using JSON3
 
Line 362 ⟶ 694:
ca, sa = cos(radians), sin(radians)
t = 1 - ca
x, y, z = Float64.(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]'] * Float64.(pointvector)
end
 
Line 375 ⟶ 707:
np = rodrotate(v1, ncp, a)
JSON3.write(np) # "[2.2322210733082284,1.3951381708176411,-8.370829024905854]"
</syntaxhighlight>
</lang>
 
=={{header|Nim}}==
{{trans|Wren}}
Only changed most function names.
<langsyntaxhighlight Nimlang="nim">import math
 
type
Line 421 ⟶ 753:
nvp = normalized(vp)
np = v1.rotate(nvp, a)
echo np</langsyntaxhighlight>
 
{{out}}
Line 427 ⟶ 759:
 
=={{header|Perl}}==
===Task-specific===
<lang perl>
<syntaxhighlight lang="perl">#!perl -w
use strict;
use Math::Trig; # acos
Line 484 ⟶ 816:
my $json=JSON->new->canonical;
 
print $json->encode($np) . "\n"; # =</syntaxhighlight> [ 2.23222107330823, 1.39513817081764, -8.37082902490585 ] = ok.
{{out}}
</lang>
<pre>[2.23222107330823,1.39513817081764,-8.37082902490585]</pre>
===Generalized===
<syntaxhighlight lang="perl">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};</syntaxhighlight>
{{out}}
<pre>2.23222107330823 1.39513817081764 -8.37082902490585</pre>
 
=={{header|Phix}}==
{{trans|JavaScript}}
<!--<syntaxhighlight lang="phix">(phixonline)-->
<span style="color: #008080;">with</span> <span style="color: #008080;">javascript_semantics</span>
<span style="color: #008080;">function</span> <span style="color: #000000;">norm</span><span style="color: #0000FF;">(</span><span style="color: #004080;">sequence</span> <span style="color: #000000;">v</span><span style="color: #0000FF;">)</span>
<span style="color: #008080;">return</span> <span style="color: #7060A8;">sqrt</span><span style="color: #0000FF;">(</span><span style="color: #7060A8;">sum</span><span style="color: #0000FF;">(</span><span style="color: #7060A8;">sq_power</span><span style="color: #0000FF;">(</span><span style="color: #000000;">v</span><span style="color: #0000FF;">,</span><span style="color: #000000;">2</span><span style="color: #0000FF;">)))</span>
<span style="color: #008080;">end</span> <span style="color: #008080;">function</span>
<span style="color: #008080;">function</span> <span style="color: #000000;">normalize</span><span style="color: #0000FF;">(</span><span style="color: #004080;">sequence</span> <span style="color: #000000;">v</span><span style="color: #0000FF;">)</span>
<span style="color: #008080;">return</span> <span style="color: #7060A8;">sq_div</span><span style="color: #0000FF;">(</span><span style="color: #000000;">v</span><span style="color: #0000FF;">,</span><span style="color: #000000;">norm</span><span style="color: #0000FF;">(</span><span style="color: #000000;">v</span><span style="color: #0000FF;">))</span>
<span style="color: #008080;">end</span> <span style="color: #008080;">function</span>
<span style="color: #008080;">function</span> <span style="color: #000000;">dotProduct</span><span style="color: #0000FF;">(</span><span style="color: #004080;">sequence</span> <span style="color: #000000;">v1</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">v2</span><span style="color: #0000FF;">)</span>
<span style="color: #008080;">return</span> <span style="color: #7060A8;">sum</span><span style="color: #0000FF;">(</span><span style="color: #7060A8;">sq_mul</span><span style="color: #0000FF;">(</span><span style="color: #000000;">v1</span><span style="color: #0000FF;">,</span><span style="color: #000000;">v2</span><span style="color: #0000FF;">))</span>
<span style="color: #008080;">end</span> <span style="color: #008080;">function</span>
<span style="color: #008080;">function</span> <span style="color: #000000;">crossProduct</span><span style="color: #0000FF;">(</span><span style="color: #004080;">sequence</span> <span style="color: #000000;">v1</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">v2</span><span style="color: #0000FF;">)</span>
<span style="color: #004080;">atom</span> <span style="color: #0000FF;">{</span><span style="color: #000000;">v11</span><span style="color: #0000FF;">,</span><span style="color: #000000;">v12</span><span style="color: #0000FF;">,</span><span style="color: #000000;">v13</span><span style="color: #0000FF;">}</span> <span style="color: #0000FF;">=</span> <span style="color: #000000;">v1</span><span style="color: #0000FF;">,</span>
<span style="color: #0000FF;">{</span><span style="color: #000000;">v21</span><span style="color: #0000FF;">,</span><span style="color: #000000;">v22</span><span style="color: #0000FF;">,</span><span style="color: #000000;">v23</span><span style="color: #0000FF;">}</span> <span style="color: #0000FF;">=</span> <span style="color: #000000;">v2</span>
<span style="color: #008080;">return</span> <span style="color: #0000FF;">{</span><span style="color: #000000;">v12</span><span style="color: #0000FF;">*</span><span style="color: #000000;">v23</span><span style="color: #0000FF;">-</span><span style="color: #000000;">v13</span><span style="color: #0000FF;">*</span><span style="color: #000000;">v22</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">v13</span><span style="color: #0000FF;">*</span><span style="color: #000000;">v21</span><span style="color: #0000FF;">-</span><span style="color: #000000;">v11</span><span style="color: #0000FF;">*</span><span style="color: #000000;">v23</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">v11</span><span style="color: #0000FF;">*</span><span style="color: #000000;">v22</span><span style="color: #0000FF;">-</span><span style="color: #000000;">v12</span><span style="color: #0000FF;">*</span><span style="color: #000000;">v21</span><span style="color: #0000FF;">}</span>
<span style="color: #008080;">end</span> <span style="color: #008080;">function</span>
<span style="color: #008080;">function</span> <span style="color: #000000;">getAngle</span><span style="color: #0000FF;">(</span><span style="color: #004080;">sequence</span> <span style="color: #000000;">v1</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">v2</span><span style="color: #0000FF;">)</span>
<span style="color: #008080;">return</span> <span style="color: #7060A8;">arccos</span><span style="color: #0000FF;">(</span><span style="color: #000000;">dotProduct</span><span style="color: #0000FF;">(</span><span style="color: #000000;">v1</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">v2</span><span style="color: #0000FF;">)</span> <span style="color: #0000FF;">/</span> <span style="color: #0000FF;">(</span><span style="color: #000000;">norm</span><span style="color: #0000FF;">(</span><span style="color: #000000;">v1</span><span style="color: #0000FF;">)*</span><span style="color: #000000;">norm</span><span style="color: #0000FF;">(</span><span style="color: #000000;">v2</span><span style="color: #0000FF;">)))</span>
<span style="color: #008080;">end</span> <span style="color: #008080;">function</span>
<span style="color: #008080;">function</span> <span style="color: #000000;">matrixMultiply</span><span style="color: #0000FF;">(</span><span style="color: #004080;">sequence</span> <span style="color: #000000;">matrix</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">v</span><span style="color: #0000FF;">)</span>
<span style="color: #008080;">return</span> <span style="color: #0000FF;">{</span><span style="color: #000000;">dotProduct</span><span style="color: #0000FF;">(</span><span style="color: #000000;">matrix</span><span style="color: #0000FF;">[</span><span style="color: #000000;">1</span><span style="color: #0000FF;">],</span> <span style="color: #000000;">v</span><span style="color: #0000FF;">),</span> <span style="color: #000000;">dotProduct</span><span style="color: #0000FF;">(</span><span style="color: #000000;">matrix</span><span style="color: #0000FF;">[</span><span style="color: #000000;">2</span><span style="color: #0000FF;">],</span> <span style="color: #000000;">v</span><span style="color: #0000FF;">),</span> <span style="color: #000000;">dotProduct</span><span style="color: #0000FF;">(</span><span style="color: #000000;">matrix</span><span style="color: #0000FF;">[</span><span style="color: #000000;">3</span><span style="color: #0000FF;">],</span> <span style="color: #000000;">v</span><span style="color: #0000FF;">)}</span>
<span style="color: #008080;">end</span> <span style="color: #008080;">function</span>
<span style="color: #008080;">function</span> <span style="color: #000000;">aRotate</span><span style="color: #0000FF;">(</span><span style="color: #004080;">sequence</span> <span style="color: #000000;">p</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">v</span><span style="color: #0000FF;">,</span> <span style="color: #004080;">atom</span> <span style="color: #000000;">a</span><span style="color: #0000FF;">)</span>
<span style="color: #004080;">atom</span> <span style="color: #000000;">ca</span> <span style="color: #0000FF;">=</span> <span style="color: #7060A8;">cos</span><span style="color: #0000FF;">(</span><span style="color: #000000;">a</span><span style="color: #0000FF;">),</span> <span style="color: #000000;">sa</span> <span style="color: #0000FF;">=</span> <span style="color: #7060A8;">sin</span><span style="color: #0000FF;">(</span><span style="color: #000000;">a</span><span style="color: #0000FF;">),</span> <span style="color: #000000;">t</span><span style="color: #0000FF;">=</span><span style="color: #000000;">1</span><span style="color: #0000FF;">-</span><span style="color: #000000;">ca</span><span style="color: #0000FF;">,</span> <span style="color: #0000FF;">{</span><span style="color: #000000;">x</span><span style="color: #0000FF;">,</span><span style="color: #000000;">y</span><span style="color: #0000FF;">,</span><span style="color: #000000;">z</span><span style="color: #0000FF;">}</span> <span style="color: #0000FF;">=</span><span style="color: #000000;">v</span>
<span style="color: #004080;">sequence</span> <span style="color: #000000;">r</span> <span style="color: #0000FF;">=</span> <span style="color: #0000FF;">{{</span><span style="color: #000000;">ca</span> <span style="color: #0000FF;">+</span> <span style="color: #000000;">x</span><span style="color: #0000FF;">*</span><span style="color: #000000;">x</span><span style="color: #0000FF;">*</span><span style="color: #000000;">t</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">x</span><span style="color: #0000FF;">*</span><span style="color: #000000;">y</span><span style="color: #0000FF;">*</span><span style="color: #000000;">t</span> <span style="color: #0000FF;">-</span> <span style="color: #000000;">z</span><span style="color: #0000FF;">*</span><span style="color: #000000;">sa</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">x</span><span style="color: #0000FF;">*</span><span style="color: #000000;">z</span><span style="color: #0000FF;">*</span><span style="color: #000000;">t</span> <span style="color: #0000FF;">+</span> <span style="color: #000000;">y</span><span style="color: #0000FF;">*</span><span style="color: #000000;">sa</span><span style="color: #0000FF;">},</span>
<span style="color: #0000FF;">{</span><span style="color: #000000;">x</span><span style="color: #0000FF;">*</span><span style="color: #000000;">y</span><span style="color: #0000FF;">*</span><span style="color: #000000;">t</span> <span style="color: #0000FF;">+</span> <span style="color: #000000;">z</span><span style="color: #0000FF;">*</span><span style="color: #000000;">sa</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">ca</span> <span style="color: #0000FF;">+</span> <span style="color: #000000;">y</span><span style="color: #0000FF;">*</span><span style="color: #000000;">y</span><span style="color: #0000FF;">*</span><span style="color: #000000;">t</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">y</span><span style="color: #0000FF;">*</span><span style="color: #000000;">z</span><span style="color: #0000FF;">*</span><span style="color: #000000;">t</span> <span style="color: #0000FF;">-</span> <span style="color: #000000;">x</span><span style="color: #0000FF;">*</span><span style="color: #000000;">sa</span><span style="color: #0000FF;">},</span>
<span style="color: #0000FF;">{</span><span style="color: #000000;">z</span><span style="color: #0000FF;">*</span><span style="color: #000000;">x</span><span style="color: #0000FF;">*</span><span style="color: #000000;">t</span> <span style="color: #0000FF;">-</span> <span style="color: #000000;">y</span><span style="color: #0000FF;">*</span><span style="color: #000000;">sa</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">z</span><span style="color: #0000FF;">*</span><span style="color: #000000;">y</span><span style="color: #0000FF;">*</span><span style="color: #000000;">t</span> <span style="color: #0000FF;">+</span> <span style="color: #000000;">x</span><span style="color: #0000FF;">*</span><span style="color: #000000;">sa</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">ca</span> <span style="color: #0000FF;">+</span> <span style="color: #000000;">z</span><span style="color: #0000FF;">*</span><span style="color: #000000;">z</span><span style="color: #0000FF;">*</span><span style="color: #000000;">t</span><span style="color: #0000FF;">}}</span>
<span style="color: #008080;">return</span> <span style="color: #000000;">matrixMultiply</span><span style="color: #0000FF;">(</span><span style="color: #000000;">r</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">p</span><span style="color: #0000FF;">)</span>
<span style="color: #008080;">end</span> <span style="color: #008080;">function</span>
<span style="color: #004080;">sequence</span> <span style="color: #000000;">v1</span> <span style="color: #0000FF;">=</span> <span style="color: #0000FF;">{</span><span style="color: #000000;">5</span><span style="color: #0000FF;">,-</span><span style="color: #000000;">6</span><span style="color: #0000FF;">,</span><span style="color: #000000;">4</span><span style="color: #0000FF;">},</span>
<span style="color: #000000;">v2</span> <span style="color: #0000FF;">=</span> <span style="color: #0000FF;">{</span><span style="color: #000000;">8</span><span style="color: #0000FF;">,</span><span style="color: #000000;">5</span><span style="color: #0000FF;">,-</span><span style="color: #000000;">30</span><span style="color: #0000FF;">};</span>
<span style="color: #004080;">atom</span> <span style="color: #000000;">a</span> <span style="color: #0000FF;">=</span> <span style="color: #000000;">getAngle</span><span style="color: #0000FF;">(</span><span style="color: #000000;">v1</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">v2</span><span style="color: #0000FF;">)</span>
<span style="color: #004080;">sequence</span> <span style="color: #000000;">cp</span> <span style="color: #0000FF;">=</span> <span style="color: #000000;">crossProduct</span><span style="color: #0000FF;">(</span><span style="color: #000000;">v1</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">v2</span><span style="color: #0000FF;">),</span>
<span style="color: #000000;">ncp</span> <span style="color: #0000FF;">=</span> <span style="color: #000000;">normalize</span><span style="color: #0000FF;">(</span><span style="color: #000000;">cp</span><span style="color: #0000FF;">),</span>
<span style="color: #000000;">np</span> <span style="color: #0000FF;">=</span> <span style="color: #000000;">aRotate</span><span style="color: #0000FF;">(</span><span style="color: #000000;">v1</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">ncp</span><span style="color: #0000FF;">,</span> <span style="color: #000000;">a</span><span style="color: #0000FF;">);</span>
<span style="color: #0000FF;">?</span><span style="color: #000000;">np</span>
<!--</syntaxhighlight>-->
{{out}}
<pre>
{2.232221073,1.395138171,-8.370829025}
</pre>
 
=={{header|Processing}}==
{{trans|C}}
<syntaxhighlight lang="java">
//Aamrun, 30th June 2022
 
class Vector{
private double x, y, z;
 
public Vector(double x1,double y1,double z1){
x = x1;
y = y1;
z = z1;
}
void printVector(int x,int y){
text("( " + this.x + " ) \u00ee + ( " + this.y + " ) + \u0135 ( " + this.z + ") \u006b\u0302",x,y);
}
 
public double norm() {
return Math.sqrt(this.x*this.x + this.y*this.y + this.z*this.z);
}
public Vector normalize(){
double length = this.norm();
return new Vector(this.x / length, this.y / length, this.z / length);
}
public double dotProduct(Vector v2) {
return this.x*v2.x + this.y*v2.y + this.z*v2.z;
}
public Vector crossProduct(Vector v2) {
return new Vector(this.y*v2.z - this.z*v2.y, this.z*v2.x - this.x*v2.z, this.x*v2.y - this.y*v2.x);
}
public double getAngle(Vector v2) {
return Math.acos(this.dotProduct(v2) / (this.norm()*v2.norm()));
}
public Vector aRotate(Vector v, double a) {
double ca = Math.cos(a), sa = Math.sin(a);
double t = 1.0 - ca;
double x = v.x, y = v.y, z = v.z;
Vector[] r = {
new Vector(ca + x*x*t, x*y*t - z*sa, x*z*t + y*sa),
new Vector(x*y*t + z*sa, ca + y*y*t, y*z*t - x*sa),
new Vector(z*x*t - y*sa, z*y*t + x*sa, ca + z*z*t)
};
return new Vector(this.dotProduct(r[0]), this.dotProduct(r[1]), this.dotProduct(r[2]));
}
}
 
void setup(){
Vector v1 = new Vector(5d, -6d, 4d),v2 = new Vector(8d, 5d, -30d);
double a = v1.getAngle(v2);
Vector cp = v1.crossProduct(v2);
Vector normCP = cp.normalize();
Vector np = v1.aRotate(normCP,a);
size(1200,600);
fill(#000000);
textSize(30);
text("v1 = ",10,100);
v1.printVector(60,100);
text("v2 = ",10,150);
v2.printVector(60,150);
text("rV = ",10,200);
np.printVector(60,200);
}
 
</syntaxhighlight>
 
=={{header|Raku}}==
<syntaxhighlight lang="raku" perl6line>sub infix:<⋅> { [+] @^a »×« @^b }
sub norm (@v) { sqrt @v⋅@v }
sub normalize (@v) { @v X/ @v.&norm }
Line 511 ⟶ 1,004:
my @v1 = [5,-6, 4];
my @v2 = [8, 5,-30];
say join ' ', aRotate @v1, normalize(crossProduct @v1, @v2), getAngle @v1, @v2;</langsyntaxhighlight>
{{out}}
<pre>2.232221073308229 1.3951381708176411 -8.370829024905852</pre>
 
Alternately, differing mostly in style:
 
<syntaxhighlight lang="raku" line>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</syntaxhighlight>
{{out}}
<pre>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</pre>
 
=={{header|RPL}}==
This is a direct transcription from Wikipedia's formula.
≪ DEG SWAP DUP ABS / <span style="color:grey">@ set degrees mode and normalize k</span>
→ v θ k
≪ v θ COS *
k v CROSS θ SIN * +
k DUP v DOT * 1 θ COS - * +
→NUM <span style="color:grey">@ can be removed if using HP-28/48 ROM versions</span>
≫ ≫ '<span style="color:blue">ROTV</span>' STO <span style="color:grey">@ ''( vector axis-vector angle → rotated-vector )''</span>
 
[-1 2 .4] [0 2 1] 18 <span style="color:blue">ROTV</span>
{{out}}
<pre>
[-1.11689243765 1.85005696279 .699886074428]
</pre>
 
=={{header|Wren}}==
{{trans|JavaScript}}
<langsyntaxhighlight ecmascriptlang="wren">var norm = Fn.new { |v| (v[0]*v[0] + v[1]*v[1] + v[2]*v[2]).sqrt }
 
var normalize = Fn.new { |v|
Line 557 ⟶ 1,144:
var ncp = normalize.call(cp)
var np = aRotate.call(v1, ncp, a)
System.print(np)</langsyntaxhighlight>
 
{{out}}
PureFox
9,476

edits

Retrieved from "https://rosettacode.org/wiki/Special:MobileDiff/316338...355089"