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