rigid.cpp 10 KB

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