doxygen: intern/itasc tagged
[blender.git] / intern / itasc / kdl / frames.cpp
1 /** \file itasc/kdl/frames.cpp
2  *  \ingroup itasc
3  */
4 /***************************************************************************
5                         frames.cxx -  description
6                        -------------------------
7     begin                : June 2006
8     copyright            : (C) 2006 Erwin Aertbelien
9     email                : firstname.lastname@mech.kuleuven.ac.be
10
11  History (only major changes)( AUTHOR-Description ) :
12
13  ***************************************************************************
14  *   This library is free software; you can redistribute it and/or         *
15  *   modify it under the terms of the GNU Lesser General Public            *
16  *   License as published by the Free Software Foundation; either          *
17  *   version 2.1 of the License, or (at your option) any later version.    *
18  *                                                                         *
19  *   This library is distributed in the hope that it will be useful,       *
20  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
21  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
22  *   Lesser General Public License for more details.                       *
23  *                                                                         *
24  *   You should have received a copy of the GNU Lesser General Public      *
25  *   License along with this library; if not, write to the Free Software   *
26  *   Foundation, Inc., 51 Franklin Street,                                    *
27  *   Fifth Floor, Boston, MA 02110-1301, USA.                               *
28  *                                                                         *
29  ***************************************************************************/
30
31 #include "frames.hpp"
32
33 namespace KDL {
34
35 #ifndef KDL_INLINE
36 #include "frames.inl"
37 #endif
38
39 void Frame::Make4x4(double * d)
40 {
41     int i;
42     int j;
43     for (i=0;i<3;i++) {
44         for (j=0;j<3;j++)
45             d[i*4+j]=M(i,j);
46         d[i*4+3] = p(i)/1000;
47     }
48     for (j=0;j<3;j++)
49         d[12+j] = 0.;
50     d[15] = 1;
51 }
52
53 Frame Frame::DH_Craig1989(double a,double alpha,double d,double theta)
54 // returns Modified Denavit-Hartenberg parameters (According to Craig)
55 {
56     double ct,st,ca,sa;
57     ct = cos(theta);
58     st = sin(theta);
59     sa = sin(alpha);
60     ca = cos(alpha);
61     return Frame(Rotation(
62                     ct,       -st,     0,
63                     st*ca,  ct*ca,   -sa,
64                     st*sa,  ct*sa,    ca   ),
65                  Vector(
66                     a,      -sa*d,  ca*d   )
67             );
68 }
69
70 Frame Frame::DH(double a,double alpha,double d,double theta)
71 // returns Denavit-Hartenberg parameters (Non-Modified DH)
72 {
73     double ct,st,ca,sa;
74     ct = cos(theta);
75     st = sin(theta);
76     sa = sin(alpha);
77     ca = cos(alpha);
78     return Frame(Rotation(
79                     ct,    -st*ca,   st*sa,
80                     st,     ct*ca,  -ct*sa,
81                      0,        sa,      ca   ),
82                  Vector(
83                     a*ct,      a*st,  d   )
84             );
85 }
86
87 double Vector2::Norm() const
88 {
89     double tmp0 = fabs(data[0]);
90     double tmp1 = fabs(data[1]);
91     if (tmp0 >= tmp1) {
92                 if (tmp1 == 0)
93                         return 0;
94         return tmp0*sqrt(1+sqr(tmp1/tmp0));
95     } else {
96         return tmp1*sqrt(1+sqr(tmp0/tmp1));
97     }
98 }
99 // makes v a unitvector and returns the norm of v.
100 // if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
101 // if this is not good, check the return value of this method.
102 double Vector2::Normalize(double eps) {
103         double v = this->Norm();
104         if (v < eps) {
105                 *this = Vector2(1,0);
106                 return v;
107         } else {
108                 *this = (*this)/v;
109                 return v;
110         }
111 }
112
113
114 // do some effort not to lose precision
115 double Vector::Norm() const
116 {
117     double tmp1;
118     double tmp2;
119     tmp1 = fabs(data[0]);
120     tmp2 = fabs(data[1]);
121     if (tmp1 >= tmp2) {
122         tmp2=fabs(data[2]);
123         if (tmp1 >= tmp2) {
124                 if (tmp1 == 0) {
125                         // only to everything exactly zero case, all other are handled correctly
126                         return 0;
127                 }
128             return tmp1*sqrt(1+sqr(data[1]/data[0])+sqr(data[2]/data[0]));
129         } else {
130             return tmp2*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2]));
131         }
132     } else {
133         tmp1=fabs(data[2]);
134         if (tmp2 > tmp1) {
135             return tmp2*sqrt(1+sqr(data[0]/data[1])+sqr(data[2]/data[1]));
136         } else {
137             return tmp1*sqrt(1+sqr(data[0]/data[2])+sqr(data[1]/data[2]));
138         }
139     }
140 }
141
142 // makes v a unitvector and returns the norm of v.
143 // if v is smaller than eps, Vector(1,0,0) is returned with norm 0.
144 // if this is not good, check the return value of this method.
145 double Vector::Normalize(double eps) {
146         double v = this->Norm();
147         if (v < eps) {
148                 *this = Vector(1,0,0);
149                 return v;
150         } else {
151                 *this = (*this)/v;
152                 return v;
153         }
154 }
155
156
157 bool Equal(const Rotation& a,const Rotation& b,double eps) {
158     return (Equal(a.data[0],b.data[0],eps) &&
159             Equal(a.data[1],b.data[1],eps) &&
160             Equal(a.data[2],b.data[2],eps) &&
161             Equal(a.data[3],b.data[3],eps) &&
162             Equal(a.data[4],b.data[4],eps) &&
163             Equal(a.data[5],b.data[5],eps) &&
164             Equal(a.data[6],b.data[6],eps) &&
165             Equal(a.data[7],b.data[7],eps) &&
166             Equal(a.data[8],b.data[8],eps)    );
167 }
168
169 void Rotation::Ortho()
170 {
171         double n;
172         n=sqrt(sqr(data[0])+sqr(data[3])+sqr(data[6]));n=(n>1e-10)?1.0/n:0.0;data[0]*=n;data[3]*=n;data[6]*=n; 
173         n=sqrt(sqr(data[1])+sqr(data[4])+sqr(data[7]));n=(n>1e-10)?1.0/n:0.0;data[1]*=n;data[4]*=n;data[7]*=n; 
174         n=sqrt(sqr(data[2])+sqr(data[5])+sqr(data[8]));n=(n>1e-10)?1.0/n:0.0;data[2]*=n;data[5]*=n;data[8]*=n; 
175 }
176
177 Rotation operator *(const Rotation& lhs,const Rotation& rhs)
178 // Complexity : 27M+27A
179 {
180     return Rotation(
181         lhs.data[0]*rhs.data[0]+lhs.data[1]*rhs.data[3]+lhs.data[2]*rhs.data[6],
182         lhs.data[0]*rhs.data[1]+lhs.data[1]*rhs.data[4]+lhs.data[2]*rhs.data[7],
183         lhs.data[0]*rhs.data[2]+lhs.data[1]*rhs.data[5]+lhs.data[2]*rhs.data[8],
184         lhs.data[3]*rhs.data[0]+lhs.data[4]*rhs.data[3]+lhs.data[5]*rhs.data[6],
185         lhs.data[3]*rhs.data[1]+lhs.data[4]*rhs.data[4]+lhs.data[5]*rhs.data[7],
186         lhs.data[3]*rhs.data[2]+lhs.data[4]*rhs.data[5]+lhs.data[5]*rhs.data[8],
187         lhs.data[6]*rhs.data[0]+lhs.data[7]*rhs.data[3]+lhs.data[8]*rhs.data[6],
188         lhs.data[6]*rhs.data[1]+lhs.data[7]*rhs.data[4]+lhs.data[8]*rhs.data[7],
189         lhs.data[6]*rhs.data[2]+lhs.data[7]*rhs.data[5]+lhs.data[8]*rhs.data[8]
190     );
191
192 }
193
194
195 Rotation Rotation::RPY(double roll,double pitch,double yaw)
196     {
197         double ca1,cb1,cc1,sa1,sb1,sc1;
198         ca1 = cos(yaw); sa1 = sin(yaw);
199         cb1 = cos(pitch);sb1 = sin(pitch);
200         cc1 = cos(roll);sc1 = sin(roll);
201         return Rotation(ca1*cb1,ca1*sb1*sc1 - sa1*cc1,ca1*sb1*cc1 + sa1*sc1,
202                    sa1*cb1,sa1*sb1*sc1 + ca1*cc1,sa1*sb1*cc1 - ca1*sc1,
203                    -sb1,cb1*sc1,cb1*cc1);
204     }
205
206 // Gives back a rotation matrix specified with RPY convention
207 void Rotation::GetRPY(double& roll,double& pitch,double& yaw) const
208     {
209         if (fabs(data[6]) > 1.0 - epsilon ) {
210             roll = -sign(data[6]) * atan2(data[1], data[4]);
211             pitch= -sign(data[6]) * PI / 2;
212             yaw  = 0.0 ;
213         } else {
214             roll  = atan2(data[7], data[8]);
215             pitch = atan2(-data[6], sqrt( sqr(data[0]) +sqr(data[3]) )  );
216             yaw   = atan2(data[3], data[0]);
217         }
218     }
219
220 Rotation Rotation::EulerZYZ(double Alfa,double Beta,double Gamma) {
221         double sa,ca,sb,cb,sg,cg;
222         sa  = sin(Alfa);ca = cos(Alfa);
223         sb  = sin(Beta);cb = cos(Beta);
224         sg  = sin(Gamma);cg = cos(Gamma);
225         return Rotation( ca*cb*cg-sa*sg,     -ca*cb*sg-sa*cg,        ca*sb,
226                  sa*cb*cg+ca*sg,     -sa*cb*sg+ca*cg,        sa*sb,
227                  -sb*cg ,                sb*sg,              cb
228                 );
229
230      }
231
232
233 void Rotation::GetEulerZYZ(double& alfa,double& beta,double& gamma) const {
234         if (fabs(data[6]) < epsilon ) {
235             alfa=0.0;
236             if (data[8]>0) {
237                 beta = 0.0;
238                 gamma= atan2(-data[1],data[0]);
239             } else {
240                 beta = PI;
241                 gamma= atan2(data[1],-data[0]);
242             }
243         } else {
244             alfa=atan2(data[5], data[2]);
245             beta=atan2(sqrt( sqr(data[6]) +sqr(data[7]) ),data[8]);
246             gamma=atan2(data[7], -data[6]);
247         }
248  }
249
250 Rotation Rotation::Rot(const Vector& rotaxis,double angle) {
251     // The formula is
252     // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr))
253     // can be found by multiplying it with an arbitrary vector p
254     // and noting that this vector is rotated.
255     double ct = cos(angle);
256     double st = sin(angle);
257     double vt = 1-ct;
258     Vector rotvec = rotaxis;
259         rotvec.Normalize();
260     return Rotation(
261         ct            +  vt*rotvec(0)*rotvec(0),
262         -rotvec(2)*st +  vt*rotvec(0)*rotvec(1),
263         rotvec(1)*st  +  vt*rotvec(0)*rotvec(2),
264         rotvec(2)*st  +  vt*rotvec(1)*rotvec(0),
265         ct            +  vt*rotvec(1)*rotvec(1),
266         -rotvec(0)*st +  vt*rotvec(1)*rotvec(2),
267         -rotvec(1)*st +  vt*rotvec(2)*rotvec(0),
268         rotvec(0)*st  +  vt*rotvec(2)*rotvec(1),
269         ct            +  vt*rotvec(2)*rotvec(2)
270         );
271     }
272
273 Rotation Rotation::Rot2(const Vector& rotvec,double angle) {
274     // rotvec should be normalized !
275     // The formula is
276     // V.(V.tr) + st*[V x] + ct*(I-V.(V.tr))
277     // can be found by multiplying it with an arbitrary vector p
278     // and noting that this vector is rotated.
279     double ct = cos(angle);
280     double st = sin(angle);
281     double vt = 1-ct;
282     return Rotation(
283         ct            +  vt*rotvec(0)*rotvec(0),
284         -rotvec(2)*st +  vt*rotvec(0)*rotvec(1),
285         rotvec(1)*st  +  vt*rotvec(0)*rotvec(2),
286         rotvec(2)*st  +  vt*rotvec(1)*rotvec(0),
287         ct            +  vt*rotvec(1)*rotvec(1),
288         -rotvec(0)*st +  vt*rotvec(1)*rotvec(2),
289         -rotvec(1)*st +  vt*rotvec(2)*rotvec(0),
290         rotvec(0)*st  +  vt*rotvec(2)*rotvec(1),
291         ct            +  vt*rotvec(2)*rotvec(2)
292         );
293 }
294
295
296
297 Vector Rotation::GetRot() const
298          // Returns a vector with the direction of the equiv. axis
299          // and its norm is angle
300      {
301        Vector axis  = Vector((data[7]-data[5]),
302                              (data[2]-data[6]),
303                              (data[3]-data[1]) )/2;
304
305        double sa    = axis.Norm();
306        double ca    = (data[0]+data[4]+data[8]-1)/2.0;
307        double alfa;
308        if (sa > epsilon)
309            alfa = ::atan2(sa,ca)/sa;
310            else {
311                    if (ca < 0.0) {
312                            alfa = KDL::PI;
313                            axis.data[0] = 0.0;
314                            axis.data[1] = 0.0;
315                            axis.data[2] = 0.0;
316                            if (data[0] > 0.0) {
317                                    axis.data[0] = 1.0;
318                            } else if (data[4] > 0.0) {
319                                    axis.data[1] = 1.0;
320                            } else {
321                                    axis.data[2] = 1.0;
322                            }
323                    } else {
324                            alfa = 0.0;
325                    }
326            }
327        return axis * alfa;
328      }
329
330 Vector2 Rotation::GetXZRot() const
331 {
332         // [0,1,0] x Y
333         Vector2 axis(data[7], -data[1]);
334         double norm = axis.Normalize();
335         if (norm < epsilon) {
336                 norm = (data[4] < 0.0) ? PI : 0.0;
337         } else {
338                 norm = acos(data[4]);
339         }
340         return axis*norm;
341 }
342
343
344 /** Returns the rotation angle around the equiv. axis
345  * @param axis the rotation axis is returned in this variable
346  * @param eps :  in the case of angle == 0 : rot axis is undefined and choosen
347  *                                         to be +/- Z-axis
348  *               in the case of angle == PI : 2 solutions, positive Z-component
349  *                                            of the axis is choosen.
350  * @result returns the rotation angle (between [0..PI] )
351  * /todo :
352  *   Check corresponding routines in rframes and rrframes
353  */
354 double Rotation::GetRotAngle(Vector& axis,double eps) const {
355         double ca    = (data[0]+data[4]+data[8]-1)/2.0;
356         if (ca>1-eps) {
357                 // undefined choose the Z-axis, and angle 0
358                 axis = Vector(0,0,1);
359                 return 0;
360         }
361         if (ca < -1+eps) {
362                 // two solutions, choose a positive Z-component of the axis
363                 double z = sqrt( (data[8]+1)/2 );
364                 double x = (data[2])/2/z;
365                 double y = (data[5])/2/z;
366                 axis = Vector( x,y,z  );
367                 return PI;
368         }
369         double angle = acos(ca);
370         double sa    = sin(angle);
371         axis  = Vector((data[7]-data[5])/2/sa,
372                        (data[2]-data[6])/2/sa,
373                        (data[3]-data[1])/2/sa  );
374         return angle;
375 }
376
377 bool operator==(const Rotation& a,const Rotation& b) {
378 #ifdef KDL_USE_EQUAL
379     return Equal(a,b);
380 #else
381     return ( a.data[0]==b.data[0] &&
382              a.data[1]==b.data[1] &&
383              a.data[2]==b.data[2] &&
384              a.data[3]==b.data[3] &&
385              a.data[4]==b.data[4] &&
386              a.data[5]==b.data[5] &&
387              a.data[6]==b.data[6] &&
388              a.data[7]==b.data[7] &&
389              a.data[8]==b.data[8]  );
390 #endif
391 }
392 }