BGE: slow parent was causing scaling distortion, now use correct quaternion interpola...
[blender.git] / source / gameengine / Ketsji / KX_SG_NodeRelationships.cpp
1 /**
2  * $Id$
3  * ***** BEGIN GPL LICENSE BLOCK *****
4  *
5  * This program is free software; you can redistribute it and/or
6  * modify it under the terms of the GNU General Public License
7  * as published by the Free Software Foundation; either version 2
8  * of the License, or (at your option) any later version.
9  *
10  * This program is distributed in the hope that it will be useful,
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
13  * GNU General Public License for more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program; if not, write to the Free Software Foundation,
17  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
18  *
19  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
20  * All rights reserved.
21  *
22  * The Original Code is: all of this file.
23  *
24  * Contributor(s): none yet.
25  *
26  * ***** END GPL LICENSE BLOCK *****
27  */
28
29 #include "KX_SG_NodeRelationships.h"
30
31 #ifdef HAVE_CONFIG_H
32 #include <config.h>
33 #endif
34
35 /**
36  * Implementation of classes defined in KX_SG_NodeRelationships.h
37  */
38
39 /** 
40  * first of all KX_NormalParentRelation
41  */
42
43         KX_NormalParentRelation *
44 KX_NormalParentRelation::
45 New(
46 ) {
47         return new KX_NormalParentRelation();
48 }               
49
50         bool
51 KX_NormalParentRelation::
52 UpdateChildCoordinates(
53         SG_Spatial * child,
54         const SG_Spatial * parent,
55         bool& parentUpdated     
56 ){
57         MT_assert(child != NULL);
58
59         if (!parentUpdated && !child->IsModified())
60                 return false;
61
62         parentUpdated = true;
63
64         if (parent==NULL) { /* Simple case */
65                 child->SetWorldFromLocalTransform();
66                 child->SetModified(false);
67                 return true; //false;
68         }
69         else {
70                 // the childs world locations which we will update.     
71                 const MT_Vector3 & p_world_scale = parent->GetWorldScaling();
72                 const MT_Point3 & p_world_pos = parent->GetWorldPosition();
73                 const MT_Matrix3x3 & p_world_rotation = parent->GetWorldOrientation();
74
75                 child->SetWorldScale(p_world_scale * child->GetLocalScale());
76                 child->SetWorldOrientation(p_world_rotation * child->GetLocalOrientation());
77                 child->SetWorldPosition(p_world_pos + p_world_scale * (p_world_rotation * child->GetLocalPosition()));
78                 child->SetModified(false);
79                 return true;
80         }
81 }
82
83         SG_ParentRelation *
84 KX_NormalParentRelation::
85 NewCopy(
86 ){
87         return new KX_NormalParentRelation();
88 }
89
90 KX_NormalParentRelation::
91 ~KX_NormalParentRelation(
92 ){
93         //nothing to do
94 }
95
96
97 KX_NormalParentRelation::
98 KX_NormalParentRelation(
99 ){
100         // nothing to do
101 }
102
103 /** 
104  * Next KX_VertexParentRelation
105  */
106
107
108         KX_VertexParentRelation *
109 KX_VertexParentRelation::
110 New(
111 ){
112         return new KX_VertexParentRelation();
113 }
114                 
115 /** 
116  * Method inherited from KX_ParentRelation
117  */
118
119         bool
120 KX_VertexParentRelation::
121 UpdateChildCoordinates(
122         SG_Spatial * child,
123         const SG_Spatial * parent,
124         bool& parentUpdated     
125 ){
126
127         MT_assert(child != NULL);
128
129         if (!parentUpdated && !child->IsModified())
130                 return false;
131
132         child->SetWorldScale(child->GetLocalScale());
133         
134         if (parent)
135                 child->SetWorldPosition(child->GetLocalPosition()+parent->GetWorldPosition());
136         else
137                 child->SetWorldPosition(child->GetLocalPosition());
138         
139         child->SetWorldOrientation(child->GetLocalOrientation());
140         child->SetModified(false);
141         return true; //parent != NULL;
142 }
143
144 /** 
145  * Method inherited from KX_ParentRelation
146  */
147
148         SG_ParentRelation *
149 KX_VertexParentRelation::
150 NewCopy(
151 ){
152         return new KX_VertexParentRelation();
153 };
154
155 KX_VertexParentRelation::
156 ~KX_VertexParentRelation(
157 ){
158         //nothing to do
159 }
160
161
162 KX_VertexParentRelation::
163 KX_VertexParentRelation(
164 ){
165         //nothing to do
166 }
167
168
169 /**
170  * Slow parent relationship
171  */
172
173         KX_SlowParentRelation *
174 KX_SlowParentRelation::
175 New(
176         MT_Scalar relaxation
177 ){
178         return new      KX_SlowParentRelation(relaxation);
179 }       
180
181 /** 
182  * Method inherited from KX_ParentRelation
183  */
184
185         bool
186 KX_SlowParentRelation::
187 UpdateChildCoordinates(
188         SG_Spatial * child,
189         const SG_Spatial * parent,
190         bool& parentUpdated     
191 ){
192         MT_assert(child != NULL);
193
194         // the child will move even if the parent is not
195         parentUpdated = true;
196
197         const MT_Vector3 & child_scale = child->GetLocalScale();
198         const MT_Point3 & child_pos = child->GetLocalPosition();
199         const MT_Matrix3x3 & child_rotation = child->GetLocalOrientation();
200
201         // the childs world locations which we will update.     
202         
203         MT_Vector3 child_w_scale;
204         MT_Point3 child_w_pos;
205         MT_Matrix3x3 child_w_rotation;
206                 
207         if (parent) {
208
209                 // This is a slow parent relation
210                 // first compute the normal child world coordinates.
211
212                 MT_Vector3 child_n_scale;
213                 MT_Point3 child_n_pos;
214                 MT_Matrix3x3 child_n_rotation;
215
216                 const MT_Vector3 & p_world_scale = parent->GetWorldScaling();
217                 const MT_Point3 & p_world_pos = parent->GetWorldPosition();
218                 const MT_Matrix3x3 & p_world_rotation = parent->GetWorldOrientation();
219
220                 child_n_scale = p_world_scale * child_scale;
221                 child_n_rotation = p_world_rotation * child_rotation;
222
223                 child_n_pos = p_world_pos + p_world_scale * 
224                         (p_world_rotation * child_pos);
225
226
227                 if (m_initialized) {
228
229                         // get the current world positions
230
231                         child_w_scale = child->GetWorldScaling();
232                         child_w_pos = child->GetWorldPosition();
233                         child_w_rotation = child->GetWorldOrientation();        
234
235                         // now 'interpolate' the normal coordinates with the last 
236                         // world coordinates to get the new world coordinates.
237
238                         MT_Scalar weight = MT_Scalar(1)/(m_relax + 1);
239                         child_w_scale = (m_relax * child_w_scale + child_n_scale) * weight;
240                         child_w_pos = (m_relax * child_w_pos + child_n_pos) * weight;
241                         // for rotation we must go through quaternion
242                         MT_Quaternion child_w_quat = child_w_rotation.getRotation().slerp(child_n_rotation.getRotation(), weight);
243                         child_w_rotation.setRotation(child_w_quat);
244                         //FIXME: update physics controller.
245                 } else {
246                         child_w_scale = child_n_scale;
247                         child_w_pos = child_n_pos;
248                         child_w_rotation = child_n_rotation;
249                         m_initialized = true;
250                 }
251                         
252         } else {
253
254                 child_w_scale = child_scale;
255                 child_w_pos = child_pos;
256                 child_w_rotation = child_rotation;
257         }
258
259         child->SetWorldScale(child_w_scale);
260         child->SetWorldPosition(child_w_pos);
261         child->SetWorldOrientation(child_w_rotation);
262         child->SetModified(false);
263         
264         return true; //parent != NULL;
265 }
266
267 /** 
268  * Method inherited from KX_ParentRelation
269  */
270
271         SG_ParentRelation *
272 KX_SlowParentRelation::
273 NewCopy(
274 ){
275         return new      KX_SlowParentRelation(m_relax);
276 }
277
278 KX_SlowParentRelation::
279 KX_SlowParentRelation(
280         MT_Scalar relaxation
281 ):
282         m_relax(relaxation),
283         m_initialized(false)
284 {
285         //nothing to do
286 }
287
288 KX_SlowParentRelation::
289 ~KX_SlowParentRelation(
290 ){
291         //nothing to do
292 }
293
294
295
296