rigid.cpp
Engine/source/T3D/rigid.cpp
More...
Detailed Description
1
2//-----------------------------------------------------------------------------
3// Copyright (c) 2012 GarageGames, LLC
4//
5// Permission is hereby granted, free of charge, to any person obtaining a copy
6// of this software and associated documentation files (the "Software"), to
7// deal in the Software without restriction, including without limitation the
8// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
9// sell copies of the Software, and to permit persons to whom the Software is
10// furnished to do so, subject to the following conditions:
11//
12// The above copyright notice and this permission notice shall be included in
13// all copies or substantial portions of the Software.
14//
15// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
16// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
17// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
18// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
19// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
20// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
21// IN THE SOFTWARE.
22//-----------------------------------------------------------------------------
23
24#include "T3D/rigid.h"
25#include "console/console.h"
26
27
28//----------------------------------------------------------------------------
29
30Rigid::Rigid()
31{
32 force.set(0.0f,0.0f,0.0f);
33 torque.set(0.0f,0.0f,0.0f);
34 linVelocity.set(0.0f,0.0f,0.0f);
35 linPosition.set(0.0f,0.0f,0.0f);
36 linMomentum.set(0.0f,0.0f,0.0f);
37 angVelocity.set(0.0f,0.0f,0.0f);
38 angMomentum.set(0.0f,0.0f,0.0f);
39
40 angPosition.identity();
41 invWorldInertia.identity();
42
43 centerOfMass.set(0.0f,0.0f,0.0f);
44 worldCenterOfMass = linPosition;
45 mass = oneOverMass = 1.0f;
46 invObjectInertia.identity();
47 restitution = 0.3f;
48 friction = 0.5f;
49 atRest = false;
50}
51
52void Rigid::clearForces()
53{
54 force.set(0.0f,0.0f,0.0f);
55 torque.set(0.0f,0.0f,0.0f);
56}
57
58//-----------------------------------------------------------------------------
59void Rigid::integrate(F32 delta)
60{
61 // Update Angular position
62 F32 angle = angVelocity.len();
63 if (angle != 0.0f) {
64 QuatF dq;
65 F32 sinHalfAngle;
66 mSinCos(angle * delta * -0.5f, sinHalfAngle, dq.w);
67 sinHalfAngle *= 1.0f / angle;
68 dq.x = angVelocity.x * sinHalfAngle;
69 dq.y = angVelocity.y * sinHalfAngle;
70 dq.z = angVelocity.z * sinHalfAngle;
71 QuatF tmp = angPosition;
72 angPosition.mul(tmp, dq);
73 angPosition.normalize();
74
75 // Rotate the position around the center of mass
76 Point3F lp = linPosition - worldCenterOfMass;
77 dq.mulP(lp,&linPosition);
78 linPosition += worldCenterOfMass;
79 }
80
81 // Update angular momentum
82 angMomentum = angMomentum + torque * delta;
83
84 // Update linear position, momentum
85 linPosition = linPosition + linVelocity * delta;
86 linMomentum = linMomentum + force * delta;
87 linVelocity = linMomentum * oneOverMass;
88
89 // Update dependent state variables
90 updateInertialTensor();
91 updateVelocity();
92 updateCenterOfMass();
93}
94
95void Rigid::updateVelocity()
96{
97 linVelocity.x = linMomentum.x * oneOverMass;
98 linVelocity.y = linMomentum.y * oneOverMass;
99 linVelocity.z = linMomentum.z * oneOverMass;
100 invWorldInertia.mulV(angMomentum,&angVelocity);
101}
102
103void Rigid::updateInertialTensor()
104{
105 MatrixF iv,qmat;
106 angPosition.setMatrix(&qmat);
107 iv.mul(qmat,invObjectInertia);
108 qmat.transpose();
109 invWorldInertia.mul(iv,qmat);
110}
111
112void Rigid::updateCenterOfMass()
113{
114 // Move the center of mass into world space
115 angPosition.mulP(centerOfMass,&worldCenterOfMass);
116 worldCenterOfMass += linPosition;
117}
118
119void Rigid::applyImpulse(const Point3F &r, const Point3F &impulse)
120{
121 atRest = false;
122
123 // Linear momentum and velocity
124 linMomentum += impulse;
125 linVelocity.x = linMomentum.x * oneOverMass;
126 linVelocity.y = linMomentum.y * oneOverMass;
127 linVelocity.z = linMomentum.z * oneOverMass;
128
129 // Rotational momentum and velocity
130 Point3F tv;
131 mCross(r,impulse,&tv);
132 angMomentum += tv;
133 invWorldInertia.mulV(angMomentum, &angVelocity);
134}
135
136//-----------------------------------------------------------------------------
137/** Resolve collision with another rigid body
138 Computes & applies the collision impulses needed to keep the bodies
139 from interpenetrating.
140
141 tg: This function was commented out... I uncommented it, but haven't
142 double checked the math.
143*/
144bool Rigid::resolveCollision(const Point3F& p, const Point3F &normal, Rigid* rigid)
145{
146 atRest = false;
147 Point3F v1,v2,r1,r2;
148 getOriginVector(p,&r1);
149 getVelocity(r1,&v1);
150 rigid->getOriginVector(p,&r2);
151 rigid->getVelocity(r2,&v2);
152
153 // Make sure they are converging
154 F32 nv = mDot(v1,normal);
155 nv -= mDot(v2,normal);
156 if (nv > 0.0f)
157 return false;
158
159 // Compute impulse
160 F32 d, n = -nv * (2.0f + restitution * rigid->restitution);
161 Point3F a1,b1,c1;
162 mCross(r1,normal,&a1);
163 invWorldInertia.mulV(a1,&b1);
164 mCross(b1,r1,&c1);
165
166 Point3F a2,b2,c2;
167 mCross(r2,normal,&a2);
168 rigid->invWorldInertia.mulV(a2,&b2);
169 mCross(b2,r2,&c2);
170
171 Point3F c3 = c1 + c2;
172 d = oneOverMass + rigid->oneOverMass + mDot(c3,normal);
173 Point3F impulse = normal * (n / d);
174
175 applyImpulse(r1,impulse);
176 impulse.neg();
177 rigid->applyImpulse(r2, impulse);
178 return true;
179}
180
181//-----------------------------------------------------------------------------
182/** Resolve collision with an immovable object
183 Computes & applies the collision impulse needed to keep the body
184 from penetrating the given surface.
185*/
186bool Rigid::resolveCollision(const Point3F& p, const Point3F &normal)
187{
188 atRest = false;
189 Point3F v,r;
190 getOriginVector(p,&r);
191 getVelocity(r,&v);
192 F32 n = -mDot(v,normal);
193 if (n >= 0.0f) {
194
195 // Collision impulse, straight forward force stuff.
196 F32 d = getZeroImpulse(r,normal);
197 F32 j = n * (1.0f + restitution) * d;
198 Point3F impulse = normal * j;
199
200 // Friction impulse, calculated as a function of the
201 // amount of force it would take to stop the motion
202 // perpendicular to the normal.
203 Point3F uv = v + (normal * n);
204 F32 ul = uv.len();
205 if (ul) {
206 uv /= -ul;
207 F32 u = ul * getZeroImpulse(r,uv);
208 j *= friction;
209 if (u > j)
210 u = j;
211 impulse += uv * u;
212 }
213
214 //
215 applyImpulse(r,impulse);
216 }
217 return true;
218}
219
220//-----------------------------------------------------------------------------
221/** Calculate the inertia along the given vector
222 This function can be used to calculate the amount of force needed to
223 affect a change in velocity along the specified normal applied at
224 the given point.
225*/
226F32 Rigid::getZeroImpulse(const Point3F& r,const Point3F& normal)
227{
228 Point3F a,b,c;
229 mCross(r,normal,&a);
230 invWorldInertia.mulV(a,&b);
231 mCross(b,r,&c);
232 return 1 / (oneOverMass + mDot(c,normal));
233}
234
235F32 Rigid::getKineticEnergy()
236{
237 Point3F w;
238 QuatF qmat = angPosition;
239 qmat.inverse();
240 qmat.mulP(angVelocity,&w);
241 const F32* f = invObjectInertia;
242 return 0.5f * ((mass * mDot(linVelocity,linVelocity)) +
243 w.x * w.x / f[0] +
244 w.y * w.y / f[5] +
245 w.z * w.z / f[10]);
246}
247
248void Rigid::getOriginVector(const Point3F &p,Point3F* r)
249{
250 *r = p - worldCenterOfMass;
251}
252
253void Rigid::setCenterOfMass(const Point3F &newCenter)
254{
255 // Sets the center of mass relative to the origin.
256 centerOfMass = newCenter;
257
258 // Update world center of mass
259 angPosition.mulP(centerOfMass,&worldCenterOfMass);
260 worldCenterOfMass += linPosition;
261}
262
263void Rigid::translateCenterOfMass(const Point3F &oldPos,const Point3F &newPos)
264{
265 // I + mass * (crossmatrix(centerOfMass)^2 - crossmatrix(newCenter)^2)
266 MatrixF oldx,newx;
267 oldx.setCrossProduct(oldPos);
268 newx.setCrossProduct(newPos);
269 for (S32 row = 0; row < 3; row++)
270 for (S32 col = 0; col < 3; col++) {
271 F32 n = newx(row,col), o = oldx(row,col);
272 objectInertia(row,col) += mass * ((o * o) - (n * n));
273 }
274
275 // Make sure the matrix is symetrical
276 objectInertia(1,0) = objectInertia(0,1);
277 objectInertia(2,0) = objectInertia(0,2);
278 objectInertia(2,1) = objectInertia(1,2);
279}
280
281void Rigid::getVelocity(const Point3F& r, Point3F* v)
282{
283 mCross(angVelocity, r, v);
284 *v += linVelocity;
285}
286
287void Rigid::getTransform(MatrixF* mat)
288{
289 angPosition.setMatrix(mat);
290 mat->setColumn(3,linPosition);
291}
292
293void Rigid::setTransform(const MatrixF& mat)
294{
295 angPosition.set(mat);
296 mat.getColumn(3,&linPosition);
297
298 // Update center of mass
299 angPosition.mulP(centerOfMass,&worldCenterOfMass);
300 worldCenterOfMass += linPosition;
301}
302
303
304//----------------------------------------------------------------------------
305/** Set the rigid body moment of inertia
306 The moment is calculated as a box with the given dimensions.
307*/
308void Rigid::setObjectInertia(const Point3F& r)
309{
310 // Rotational moment of inertia of a box
311 F32 ot = mass / 12.0f;
312 F32 a = r.x * r.x;
313 F32 b = r.y * r.y;
314 F32 c = r.z * r.z;
315
316 objectInertia.identity();
317 F32* f = objectInertia;
318 f[0] = ot * (b + c);
319 f[5] = ot * (c + a);
320 f[10] = ot * (a + b);
321
322 invertObjectInertia();
323 updateInertialTensor();
324}
325
326
327//----------------------------------------------------------------------------
328/** Set the rigid body moment of inertia
329 The moment is calculated as a unit sphere.
330*/
331void Rigid::setObjectInertia()
332{
333 objectInertia.identity();
334 F32 radius = 1.0f;
335 F32* f = objectInertia;
336 f[0] = f[5] = f[10] = (0.4f * mass * radius * radius);
337 invertObjectInertia();
338 updateInertialTensor();
339}
340
341void Rigid::invertObjectInertia()
342{
343 invObjectInertia = objectInertia;
344 invObjectInertia.fullInverse();
345}
346
347
348//----------------------------------------------------------------------------
349
350bool Rigid::checkRestCondition()
351{
352// F32 k = getKineticEnergy(mWorldToObj);
353// F32 G = -force.z * oneOverMass * 0.032;
354// F32 Kg = 0.5 * mRigid.mass * G * G;
355// if (k < Kg * restTol)
356// mRigid.setAtRest();
357 return atRest;
358}
359
360void Rigid::setAtRest()
361{
362 atRest = true;
363 linVelocity.set(0.0f,0.0f,0.0f);
364 linMomentum.set(0.0f,0.0f,0.0f);
365 angVelocity.set(0.0f,0.0f,0.0f);
366 angMomentum.set(0.0f,0.0f,0.0f);
367}
368
