Rodrigues’ rotation formula: Difference between revisions
Content added Content deleted
(add FreeBASIC) |
|||
Line 171: | Line 171: | ||
<pre> |
<pre> |
||
{ 2.232221073308229 1.395138170817642 -8.370829024905852 } |
{ 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. |
|||
<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> |
|||
{{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> |
</pre> |
||