Rodrigues’ rotation formula: Difference between revisions
(Added Processing implementation) |
(Added credits) |
||
Line 908:
=={{header|Processing}}==
{{trans|C}}
<lang java>
//Aamrun, 30th June 2022
|
Revision as of 19:58, 30 June 2022
- Task
Rotate a point about some axis by some angle using Rodrigues' rotation formula.
- Reference
Ada
<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; </lang>
- Output:
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]
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 )
AutoHotkey
<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)
}</lang>
- Output:
[2.232221, 1.395138, -8.370829]
C
<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;
}</lang>
- Output:
[2.2322210733082, 1.3951381708176, -8.3708290249059]
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() |
<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 }
FreeBASIC
This example rotates the vector [-1, 2, -0.4] around the axis [-1, 2, 1] in increments of 18 degrees. <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</lang>
- Output:
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]
Go
<lang go>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)
}</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) ]; };
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 );
})();</lang>
- Output:
[ 2.2322210733082275, 1.3951381708176431, -8.370829024905852 ]
jq
Adapted from Wren
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. <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</lang>
- Output:
[2.2322210733082275,1.3951381708176436,-8.370829024905852]
Julia
<lang julia>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]" </lang>
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
Task-specific
<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";</lang>
- Output:
[2.23222107330823,1.39513817081764,-8.37082902490585]
Generalized
<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};</lang>
- Output:
2.23222107330823 1.39513817081764 -8.37082902490585
Phix
with javascript_semantics function norm(sequence v) return sqrt(sum(sq_power(v,2))) end function function normalize(sequence v) return sq_div(v,norm(v)) end function function dotProduct(sequence v1, v2) return sum(sq_mul(v1,v2)) end function function crossProduct(sequence v1, v2) atom {v11,v12,v13} = v1, {v21,v22,v23} = v2 return {v12*v23-v13*v22, v13*v21-v11*v23, v11*v22-v12*v21} end function function getAngle(sequence v1, v2) return arccos(dotProduct(v1, v2) / (norm(v1)*norm(v2))) end function function matrixMultiply(sequence matrix, v) return {dotProduct(matrix[1], v), dotProduct(matrix[2], v), dotProduct(matrix[3], v)} end function function aRotate(sequence p, v, atom a) atom ca = cos(a), sa = sin(a), t=1-ca, {x,y,z} =v sequence 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) end function sequence v1 = {5,-6,4}, v2 = {8,5,-30}; atom a = getAngle(v1, v2) sequence cp = crossProduct(v1, v2), ncp = normalize(cp), np = aRotate(v1, ncp, a); ?np
- Output:
{2.232221073,1.395138171,-8.370829025}
Processing
<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);
}
</lang>
Raku
<lang perl6>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], [z×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;</lang>
- Output:
2.232221073308229 1.3951381708176411 -8.370829024905852
Alternately, differing mostly in style:
<lang perl6>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</lang>
- 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
<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]