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