Teh first one
[mldemos:kalians-mldemos.git] / _AlgorithmsPlugins / Obstacle / MathLib / ReferenceFrame.h
1 /*\r
2  * Copyright (C) 2010 Learning Algorithms and Systems Laboratory, EPFL, Switzerland\r
3  * Author: Eric Sauser\r
4  * email:   eric.sauser@a3.epf.ch\r
5  * website: lasa.epfl.ch\r
6  *\r
7  * Permission is granted to copy, distribute, and/or modify this program\r
8  * under the terms of the GNU General Public License, version 2 or any\r
9  * later version published by the Free Software Foundation.\r
10  *\r
11  * This program is distributed in the hope that it will be useful, but\r
12  * WITHOUT ANY WARRANTY; without even the implied warranty of\r
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General\r
14  * Public License for more details\r
15  */\r
16 \r
17 #ifndef __ReferenceFrame_H__\r
18 #define __ReferenceFrame_H__\r
19 \r
20 #include "MathLibCommon.h"\r
21 \r
22 #include "Vector3.h"\r
23 #include "Matrix3.h"\r
24 #include "Matrix4.h"\r
25 \r
26 #define REFFRAME_NONE       0\r
27 #define REFFRAME_BASEREF   -1\r
28 #define REFFRAME_HMATRIX   +1\r
29 \r
30 #ifdef USE_MATHLIB_NAMESPACE\r
31 namespace MathLib{\r
32 #endif\r
33 \r
34 class ReferenceFrame;\r
35 typedef ReferenceFrame *pReferenceFrame;\r
36 \r
37 /**\r
38  * \class ReferenceFrame\r
39  * \r
40  * \ingroup MathLib\r
41  * \r
42  * \brief A wrapper class to consider frame of references\r
43  * \r
44  */\r
45 \r
46 \r
47 class ReferenceFrame\r
48\r
49 protected:\r
50 \r
51   Vector3           mOrigin;\r
52   Matrix3           mOrient;\r
53   Matrix4           mHMatrix;\r
54 \r
55   ReferenceFrame   *mInverse;\r
56 \r
57   int               mInvalidRepresentation;\r
58   bool              bInverseChanged;\r
59 \r
60 public:\r
61 \r
62   inline ReferenceFrame(){\r
63     mInverse = new ReferenceFrame(this);    \r
64     Init();\r
65   }\r
66   \r
67   inline ReferenceFrame(const ReferenceFrame & referenceFrame){\r
68     ReferenceFrame();\r
69     Set(referenceFrame);\r
70   }\r
71   \r
72   inline ReferenceFrame(const Vector3 &origin, const Matrix3 &orient){\r
73     ReferenceFrame();\r
74     Set(origin,orient);\r
75   }\r
76   \r
77   inline ReferenceFrame(const Matrix4 & hMatrix){\r
78     ReferenceFrame();\r
79     SetHMatrix(hMatrix);\r
80   }\r
81 \r
82   inline ~ReferenceFrame(){\r
83     if(mInverse!=NULL){\r
84       mInverse->mInverse = NULL;\r
85       delete mInverse;\r
86       mInverse = NULL;\r
87     }\r
88   }\r
89 \r
90 \r
91   inline ReferenceFrame& Set(const ReferenceFrame& referenceFrame){    \r
92     Set(referenceFrame.mOrigin,referenceFrame.mOrient);\r
93     return *this;\r
94   }\r
95 \r
96   inline ReferenceFrame& operator = (const ReferenceFrame& referenceFrame){\r
97     return Set(referenceFrame);\r
98   }\r
99 \r
100   inline ReferenceFrame& Set(const Vector3& origin, const Matrix3& orient){\r
101     SetOrigin(origin);\r
102     SetOrient(orient);\r
103     return *this;\r
104   }\r
105   \r
106   inline ReferenceFrame& Identity(){\r
107     SetOrigin(Vector3::ZERO);\r
108     SetOrient(Matrix3::IDENTITY);\r
109     return *this;\r
110   }\r
111 \r
112 \r
113 \r
114 \r
115   inline const Vector3& GetOrigin(){\r
116     if(bInverseChanged) UpdateFromInverse();\r
117     else if (mInvalidRepresentation == REFFRAME_BASEREF)  UpdateBaseRef();\r
118     return mOrigin;\r
119   }\r
120   inline const Matrix3& GetOrient(){\r
121     if(bInverseChanged) UpdateFromInverse();\r
122     else if (mInvalidRepresentation == REFFRAME_BASEREF)  UpdateBaseRef();\r
123     return mOrient;\r
124   }\r
125   inline const Matrix4& GetHMatrix(){\r
126     if(bInverseChanged) UpdateFromInverse();\r
127     else if (mInvalidRepresentation == REFFRAME_HMATRIX)  UpdateHMatrix();\r
128     return mHMatrix;  \r
129   }\r
130 \r
131 \r
132   \r
133   inline Vector3& SetOrigin(){\r
134     mInvalidRepresentation    = REFFRAME_HMATRIX;\r
135     mInverse->bInverseChanged = true;\r
136     return mOrigin;\r
137   }\r
138   inline Matrix3& SetOrient(){\r
139     mInvalidRepresentation    = REFFRAME_HMATRIX;\r
140     mInverse->bInverseChanged = true;\r
141     return mOrient;\r
142   }\r
143   inline Matrix4& SetHMatrix(){\r
144     mInvalidRepresentation    = REFFRAME_BASEREF;\r
145     mInverse->bInverseChanged = true;\r
146     return mHMatrix;  \r
147   }\r
148 \r
149   inline ReferenceFrame& SetOrigin(const Vector3& origin){\r
150     SetOrigin().Set(origin);\r
151     return *this;\r
152   }\r
153   inline ReferenceFrame& SetOrient(const Matrix3& orient){\r
154     SetOrient().Set(orient);\r
155     return *this;\r
156   }\r
157   inline ReferenceFrame& SetHMatrix(const Matrix4& hMatrix){\r
158     SetHMatrix().Set(hMatrix);\r
159     return *this;\r
160   }\r
161 \r
162 \r
163   inline Vector3& GetSetOrigin(){\r
164     GetOrigin();\r
165     return SetOrigin();\r
166   }\r
167   inline Matrix3& GetSetOrient(){\r
168     GetOrient();\r
169     return SetOrient();\r
170   }\r
171   inline Matrix4& GetSetHMatrix(){\r
172     GetHMatrix();\r
173     return SetHMatrix();  \r
174   }\r
175   \r
176   inline const Vector3& IGetOrigin(){\r
177     return mOrigin;\r
178   }\r
179   inline const Matrix3& IGetOrient(){\r
180     return mOrient;\r
181   }\r
182   inline const Matrix4& IGetHMatrix(){\r
183     return mHMatrix;  \r
184   }\r
185   \r
186 \r
187 \r
188   inline ReferenceFrame& GetInverse(){\r
189     return *mInverse;  \r
190   }\r
191 \r
192   \r
193   inline ReferenceFrame Mult(ReferenceFrame& referenceFrame){\r
194     ReferenceFrame result;\r
195     Mult(referenceFrame,result);\r
196     return result;\r
197   }\r
198   \r
199   inline ReferenceFrame& Mult(ReferenceFrame& referenceFrame, ReferenceFrame& result){\r
200     if(bInverseChanged) UpdateFromInverse();\r
201     else if(mInvalidRepresentation == REFFRAME_BASEREF)  UpdateBaseRef();\r
202 \r
203     mOrient.Mult(referenceFrame.GetOrient(),result.mOrient);\r
204     mOrient.Mult(referenceFrame.GetOrigin(),result.mOrigin);\r
205     result.SetOrigin()+=(mOrigin);\r
206     return *this;\r
207   }\r
208   \r
209 \r
210   \r
211 \r
212 \r
213 \r
214   inline void  Update(){\r
215 \r
216     if(bInverseChanged){\r
217       UpdateFromInverse();\r
218     }else{      \r
219       if(!mInvalidRepresentation){\r
220         if(mInvalidRepresentation==REFFRAME_BASEREF){\r
221           UpdateBaseRef();\r
222           mInverse->Update();\r
223         }else{              \r
224           UpdateHMatrix();\r
225           mInverse->Update();\r
226         }\r
227       }\r
228     }\r
229   }\r
230   \r
231 \r
232 protected:\r
233   \r
234   inline void  UpdateBaseRef(){\r
235     mHMatrix.GetTranslation(mOrigin);\r
236     mHMatrix.GetOrientation(mOrient);\r
237     \r
238     mInvalidRepresentation  = REFFRAME_NONE;\r
239   }\r
240   inline void  UpdateHMatrix(){\r
241     mHMatrix.SetTransformation(mOrient,mOrigin);    \r
242     \r
243     mInvalidRepresentation  = REFFRAME_NONE;\r
244   }\r
245 \r
246   inline void UpdateFromInverse(){\r
247     if(mInverse->mInvalidRepresentation == REFFRAME_BASEREF)\r
248       mInverse->UpdateBaseRef();\r
249       \r
250     mInverse->mOrient.Transpose(mOrient);\r
251     mOrient.Mult(mInverse->mOrigin,mOrigin);\r
252     mOrigin.SMinus();\r
253     mHMatrix.SetTransformation(mOrient,mOrigin);\r
254     \r
255     mInvalidRepresentation  = REFFRAME_NONE;\r
256     bInverseChanged         = false;\r
257   }\r
258 \r
259 \r
260   \r
261  \r
262 private:\r
263 \r
264   inline ReferenceFrame(ReferenceFrame *inverse){\r
265     mInverse = inverse; \r
266     Init();\r
267   }\r
268   \r
269   inline void  Init(){\r
270     Identity();  \r
271     bInverseChanged         = false;\r
272   }\r
273 \r
274 };\r
275 \r
276 #ifdef USE_MATHLIB_NAMESPACE\r
277 }\r
278 #endif\r
279 \r
280 #endif\r