Rodrigues’ rotation formula: Difference between revisions
add FreeBASIC
(add FreeBASIC) |
|||
Line 171:
<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.
<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>
| |||