Cycles: svn merge -r39669:39870 https://svn.blender.org/svnroot/bf-blender/trunk...
[blender.git] / intern / itasc / kdl / frames_io.cpp
1 /** \file itasc/kdl/frames_io.cpp
2  *  \ingroup itasc
3  */
4
5 /***************************************************************************
6                         frames_io.h -  description
7                        -------------------------
8     begin                : June 2006
9     copyright            : (C) 2006 Erwin Aertbelien
10     email                : firstname.lastname@mech.kuleuven.ac.be
11
12  History (only major changes)( AUTHOR-Description ) :
13
14  ***************************************************************************
15  *   This library is free software; you can redistribute it and/or         *
16  *   modify it under the terms of the GNU Lesser General Public            *
17  *   License as published by the Free Software Foundation; either          *
18  *   version 2.1 of the License, or (at your option) any later version.    *
19  *                                                                         *
20  *   This library is distributed in the hope that it will be useful,       *
21  *   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
22  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU     *
23  *   Lesser General Public License for more details.                       *
24  *                                                                         *
25  *   You should have received a copy of the GNU Lesser General Public      *
26  *   License along with this library; if not, write to the Free Software   *
27  *   Foundation, Inc., 51 Franklin Street,                                    *
28  *   Fifth Floor, Boston, MA 02110-1301, USA.                               *
29  *                                                                         *
30  ***************************************************************************/
31
32 #include "utilities/error.h"
33 #include "utilities/error_stack.h"
34 #include "frames.hpp"
35 #include "frames_io.hpp"
36
37 #include <stdlib.h>
38 #include <ctype.h>
39 #include <string.h>
40 #include <iostream>
41
42 namespace KDL {
43
44
45 std::ostream& operator << (std::ostream& os,const Vector& v) {
46     os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH)<<v(1)
47        << "," << std::setw(KDL_FRAME_WIDTH) << v(2) << "]";
48     return os;
49 }
50
51 std::ostream& operator << (std::ostream& os,const Twist& v) {
52     os << "[" << std::setw(KDL_FRAME_WIDTH) << v.vel(0)
53        << "," << std::setw(KDL_FRAME_WIDTH) << v.vel(1)
54        << "," << std::setw(KDL_FRAME_WIDTH) << v.vel(2)
55        << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(0)
56        << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(1)
57        << "," << std::setw(KDL_FRAME_WIDTH) << v.rot(2)
58        << "]";
59     return os;
60 }
61
62 std::ostream& operator << (std::ostream& os,const Wrench& v) {
63     os << "[" << std::setw(KDL_FRAME_WIDTH) << v.force(0)
64        << "," << std::setw(KDL_FRAME_WIDTH) << v.force(1)
65        << "," << std::setw(KDL_FRAME_WIDTH) << v.force(2)
66        << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(0)
67        << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(1)
68        << "," << std::setw(KDL_FRAME_WIDTH) << v.torque(2)
69        << "]";
70     return os;
71 }
72
73
74 std::ostream& operator << (std::ostream& os,const Rotation& R) {
75 #ifdef KDL_ROTATION_PROPERTIES_RPY
76     double r,p,y;
77     R.GetRPY(r,p,y);
78     os << "[RPY]"<<endl;
79     os << "[";
80     os << std::setw(KDL_FRAME_WIDTH) << r << ",";
81     os << std::setw(KDL_FRAME_WIDTH) << p << ",";
82     os << std::setw(KDL_FRAME_WIDTH) << y << "]";
83 #else
84 # ifdef KDL_ROTATION_PROPERTIES_EULER
85     double z,y,x;
86     R.GetEulerZYX(z,y,x);
87     os << "[EULERZYX]"<<endl;
88     os << "[";
89     os << std::setw(KDL_FRAME_WIDTH) << z << ",";
90     os << std::setw(KDL_FRAME_WIDTH) << y << ",";
91     os << std::setw(KDL_FRAME_WIDTH) << x << "]";
92 # else
93     os << "[";
94     for (int i=0;i<=2;i++) {
95         os << std::setw(KDL_FRAME_WIDTH) << R(i,0) << "," <<
96                        std::setw(KDL_FRAME_WIDTH) << R(i,1) << "," <<
97                        std::setw(KDL_FRAME_WIDTH) << R(i,2);
98         if (i<2)
99             os << ";"<< std::endl << " ";
100         else
101             os << "]";
102     }
103 # endif
104 #endif
105     return os;
106 }
107
108 std::ostream& operator << (std::ostream& os, const Frame& T)
109 {
110     os << "[" << T.M << std::endl<< T.p << "]";
111     return os;
112 }
113
114 std::ostream& operator << (std::ostream& os,const Vector2& v) {
115     os << "[" << std::setw(KDL_FRAME_WIDTH) << v(0) << "," << std::setw(KDL_FRAME_WIDTH)<<v(1)
116        << "]";
117     return os;
118 }
119
120 // Rotation2 gives back an angle in degrees with the << and >> operators.
121 std::ostream& operator << (std::ostream& os,const Rotation2& R) {
122     os << "[" << R.GetRot()*rad2deg << "]";
123     return os;
124 }
125
126 std::ostream& operator << (std::ostream& os, const Frame2& T)
127 {
128     os << T.M << T.p;
129     return os;
130 }
131
132 std::istream& operator >> (std::istream& is,Vector& v)
133 {   IOTrace("Stream input Vector (vector or ZERO)");
134     char storage[10];
135     EatWord(is,"[]",storage,10);
136     if (storage[0]=='\0') {
137         Eat(is,'[');
138         is >> v(0);
139         Eat(is,',');
140         is >> v(1);
141         Eat(is,',');
142         is >> v(2);
143         EatEnd(is,']');
144         IOTracePop();
145         return is;
146     }
147     if (strcmp(storage,"ZERO")==0) {
148         v = Vector::Zero();
149         IOTracePop();
150         return is;
151     }
152     throw Error_Frame_Vector_Unexpected_id();
153 }
154
155 std::istream& operator >> (std::istream& is,Twist& v)
156 {   IOTrace("Stream input Twist");
157     Eat(is,'[');
158     is >> v.vel(0);
159     Eat(is,',');
160     is >> v.vel(1);
161     Eat(is,',');
162     is >> v.vel(2);
163     Eat(is,',');
164     is >> v.rot(0);
165     Eat(is,',');
166     is >> v.rot(1);
167     Eat(is,',');
168     is >> v.rot(2);
169     EatEnd(is,']');
170     IOTracePop();
171     return is;
172 }
173
174 std::istream& operator >> (std::istream& is,Wrench& v)
175 {   IOTrace("Stream input Wrench");
176     Eat(is,'[');
177     is >> v.force(0);
178     Eat(is,',');
179     is >> v.force(1);
180     Eat(is,',');
181     is >> v.force(2);
182     Eat(is,',');
183     is >> v.torque(0);
184     Eat(is,',');
185     is >> v.torque(1);
186     Eat(is,',');
187     is >> v.torque(2);
188     EatEnd(is,']');
189     IOTracePop();
190     return is;
191 }
192
193 std::istream& operator >> (std::istream& is,Rotation& r)
194 {   IOTrace("Stream input Rotation (Matrix or EULERZYX, EULERZYZ,RPY, ROT, IDENTITY)");
195     char storage[10];
196     EatWord(is,"[]",storage,10);
197     if (storage[0]=='\0') {
198         Eat(is,'[');
199         for (int i=0;i<3;i++) {
200             is >> r(i,0);
201             Eat(is,',') ;
202             is >> r(i,1);
203             Eat(is,',');
204             is >> r(i,2);
205             if (i<2)
206                 Eat(is,';');
207             else
208                 EatEnd(is,']');
209         }
210         IOTracePop();
211         return is;
212     }
213     Vector v;
214     if (strcmp(storage,"EULERZYX")==0) {
215         is >> v;
216         v=v*deg2rad;
217         r = Rotation::EulerZYX(v(0),v(1),v(2));
218         IOTracePop();
219         return is;
220     }
221     if (strcmp(storage,"EULERZYZ")==0) {
222         is >> v;
223         v=v*deg2rad;
224         r = Rotation::EulerZYZ(v(0),v(1),v(2));
225         IOTracePop();
226         return is;
227     }
228     if (strcmp(storage,"RPY")==0) {
229         is >> v;
230         v=v*deg2rad;
231         r = Rotation::RPY(v(0),v(1),v(2));
232         IOTracePop();
233         return is;
234     }
235     if (strcmp(storage,"ROT")==0) {
236         is >> v;
237         double angle;
238         Eat(is,'[');
239         is >> angle;
240         EatEnd(is,']');
241         r = Rotation::Rot(v,angle*deg2rad);
242         IOTracePop();
243         return is;
244     }
245     if (strcmp(storage,"IDENTITY")==0) {
246         r = Rotation::Identity();
247         IOTracePop();
248         return is;
249     }
250     throw Error_Frame_Rotation_Unexpected_id();
251     return is;
252 }
253
254 std::istream& operator >> (std::istream& is,Frame& T)
255 {   IOTrace("Stream input Frame (Rotation,Vector) or DH[...]");
256     char storage[10];
257     EatWord(is,"[",storage,10);
258     if (storage[0]=='\0') {
259         Eat(is,'[');
260         is >> T.M;
261         is >> T.p;
262         EatEnd(is,']');
263         IOTracePop();
264         return is;
265     }
266     if (strcmp(storage,"DH")==0) {
267         double a,alpha,d,theta;
268         Eat(is,'[');
269         is >> a;
270         Eat(is,',');
271         is >> alpha;
272         Eat(is,',');
273         is >> d;
274         Eat(is,',');
275         is >> theta;
276         EatEnd(is,']');
277         T = Frame::DH(a,alpha*deg2rad,d,theta*deg2rad);
278         IOTracePop();
279         return is;
280     }
281     throw Error_Frame_Frame_Unexpected_id();
282     return is;
283 }
284
285 std::istream& operator >> (std::istream& is,Vector2& v)
286 {   IOTrace("Stream input Vector2");
287     Eat(is,'[');
288     is >> v(0);
289     Eat(is,',');
290     is >> v(1);
291     EatEnd(is,']');
292     IOTracePop();
293     return is;
294 }
295 std::istream& operator >> (std::istream& is,Rotation2& r)
296 {   IOTrace("Stream input Rotation2");
297     Eat(is,'[');
298     double val;
299     is >> val;
300     r.Rot(val*deg2rad);
301     EatEnd(is,']');
302     IOTracePop();
303     return is;
304 }
305 std::istream& operator >> (std::istream& is,Frame2& T)
306 {   IOTrace("Stream input Frame2");
307     is >> T.M;
308     is >> T.p;
309     IOTracePop();
310     return is;
311 }
312
313 } // namespace Frame