[216] | 1 | /************************************************************************* |
---|
| 2 | * * |
---|
| 3 | * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith. * |
---|
| 4 | * All rights reserved. Email: russ@q12.org Web: www.q12.org * |
---|
| 5 | * * |
---|
| 6 | * Fast iterative solver, David Whittaker. Email: david@csworkbench.com * |
---|
| 7 | * * |
---|
| 8 | * This library is free software; you can redistribute it and/or * |
---|
| 9 | * modify it under the terms of EITHER: * |
---|
| 10 | * (1) The GNU Lesser General Public License as published by the Free * |
---|
| 11 | * Software Foundation; either version 2.1 of the License, or (at * |
---|
| 12 | * your option) any later version. The text of the GNU Lesser * |
---|
| 13 | * General Public License is included with this library in the * |
---|
| 14 | * file LICENSE.TXT. * |
---|
| 15 | * (2) The BSD-style license that is included with this library in * |
---|
| 16 | * the file LICENSE-BSD.TXT. * |
---|
| 17 | * * |
---|
| 18 | * This library is distributed in the hope that it will be useful, * |
---|
| 19 | * but WITHOUT ANY WARRANTY; without even the implied warranty of * |
---|
| 20 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * |
---|
| 21 | * LICENSE.TXT and LICENSE-BSD.TXT for more details. * |
---|
| 22 | * * |
---|
| 23 | *************************************************************************/ |
---|
| 24 | |
---|
| 25 | // This is the StepFast code by David Whittaker. This code is faster, but |
---|
| 26 | // sometimes less stable than, the original "big matrix" code. |
---|
| 27 | // Refer to the user's manual for more information. |
---|
| 28 | // Note that this source file duplicates a lot of stuff from step.cpp, |
---|
| 29 | // eventually we should move the common code to a third file. |
---|
| 30 | |
---|
| 31 | #include "objects.h" |
---|
| 32 | #include "joint.h" |
---|
| 33 | #include <ode/config.h> |
---|
| 34 | #include <ode/objects.h> |
---|
| 35 | #include <ode/odemath.h> |
---|
| 36 | #include <ode/rotation.h> |
---|
| 37 | #include <ode/timer.h> |
---|
| 38 | #include <ode/error.h> |
---|
| 39 | #include <ode/matrix.h> |
---|
| 40 | #include "lcp.h" |
---|
| 41 | #include "step.h" |
---|
| 42 | |
---|
| 43 | |
---|
| 44 | // misc defines |
---|
| 45 | |
---|
| 46 | #define ALLOCA dALLOCA16 |
---|
| 47 | |
---|
| 48 | #define RANDOM_JOINT_ORDER |
---|
| 49 | //#define FAST_FACTOR //use a factorization approximation to the LCP solver (fast, theoretically less accurate) |
---|
| 50 | #define SLOW_LCP //use the old LCP solver |
---|
| 51 | //#define NO_ISLANDS //does not perform island creation code (3~4% of simulation time), body disabling doesn't work |
---|
| 52 | //#define TIMING |
---|
| 53 | |
---|
| 54 | |
---|
| 55 | static int autoEnableDepth = 2; |
---|
| 56 | |
---|
| 57 | void dWorldSetAutoEnableDepthSF1 (dxWorld *world, int autodepth) |
---|
| 58 | { |
---|
| 59 | if (autodepth > 0) |
---|
| 60 | autoEnableDepth = autodepth; |
---|
| 61 | else |
---|
| 62 | autoEnableDepth = 0; |
---|
| 63 | } |
---|
| 64 | |
---|
| 65 | int dWorldGetAutoEnableDepthSF1 (dxWorld *world) |
---|
| 66 | { |
---|
| 67 | return autoEnableDepth; |
---|
| 68 | } |
---|
| 69 | |
---|
| 70 | //little bit of math.... the _sym_ functions assume the return matrix will be symmetric |
---|
| 71 | static void |
---|
| 72 | Multiply2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip) |
---|
| 73 | { |
---|
| 74 | int i, j; |
---|
| 75 | dReal sum, *aa, *ad, *bb, *cc; |
---|
| 76 | dIASSERT (p > 0 && A && B && C); |
---|
| 77 | bb = B; |
---|
| 78 | for (i = 0; i < p; i++) |
---|
| 79 | { |
---|
| 80 | //aa is going accross the matrix, ad down |
---|
| 81 | aa = ad = A; |
---|
| 82 | cc = C; |
---|
| 83 | for (j = i; j < p; j++) |
---|
| 84 | { |
---|
| 85 | sum = bb[0] * cc[0]; |
---|
| 86 | sum += bb[1] * cc[1]; |
---|
| 87 | sum += bb[2] * cc[2]; |
---|
| 88 | sum += bb[4] * cc[4]; |
---|
| 89 | sum += bb[5] * cc[5]; |
---|
| 90 | sum += bb[6] * cc[6]; |
---|
| 91 | *(aa++) = *ad = sum; |
---|
| 92 | ad += Askip; |
---|
| 93 | cc += 8; |
---|
| 94 | } |
---|
| 95 | bb += 8; |
---|
| 96 | A += Askip + 1; |
---|
| 97 | C += 8; |
---|
| 98 | } |
---|
| 99 | } |
---|
| 100 | |
---|
| 101 | static void |
---|
| 102 | MultiplyAdd2_sym_p8p (dReal * A, dReal * B, dReal * C, int p, int Askip) |
---|
| 103 | { |
---|
| 104 | int i, j; |
---|
| 105 | dReal sum, *aa, *ad, *bb, *cc; |
---|
| 106 | dIASSERT (p > 0 && A && B && C); |
---|
| 107 | bb = B; |
---|
| 108 | for (i = 0; i < p; i++) |
---|
| 109 | { |
---|
| 110 | //aa is going accross the matrix, ad down |
---|
| 111 | aa = ad = A; |
---|
| 112 | cc = C; |
---|
| 113 | for (j = i; j < p; j++) |
---|
| 114 | { |
---|
| 115 | sum = bb[0] * cc[0]; |
---|
| 116 | sum += bb[1] * cc[1]; |
---|
| 117 | sum += bb[2] * cc[2]; |
---|
| 118 | sum += bb[4] * cc[4]; |
---|
| 119 | sum += bb[5] * cc[5]; |
---|
| 120 | sum += bb[6] * cc[6]; |
---|
| 121 | *(aa++) += sum; |
---|
| 122 | *ad += sum; |
---|
| 123 | ad += Askip; |
---|
| 124 | cc += 8; |
---|
| 125 | } |
---|
| 126 | bb += 8; |
---|
| 127 | A += Askip + 1; |
---|
| 128 | C += 8; |
---|
| 129 | } |
---|
| 130 | } |
---|
| 131 | |
---|
| 132 | |
---|
| 133 | // this assumes the 4th and 8th rows of B are zero. |
---|
| 134 | |
---|
| 135 | static void |
---|
| 136 | Multiply0_p81 (dReal * A, dReal * B, dReal * C, int p) |
---|
| 137 | { |
---|
| 138 | int i; |
---|
| 139 | dIASSERT (p > 0 && A && B && C); |
---|
| 140 | dReal sum; |
---|
| 141 | for (i = p; i; i--) |
---|
| 142 | { |
---|
| 143 | sum = B[0] * C[0]; |
---|
| 144 | sum += B[1] * C[1]; |
---|
| 145 | sum += B[2] * C[2]; |
---|
| 146 | sum += B[4] * C[4]; |
---|
| 147 | sum += B[5] * C[5]; |
---|
| 148 | sum += B[6] * C[6]; |
---|
| 149 | *(A++) = sum; |
---|
| 150 | B += 8; |
---|
| 151 | } |
---|
| 152 | } |
---|
| 153 | |
---|
| 154 | |
---|
| 155 | // this assumes the 4th and 8th rows of B are zero. |
---|
| 156 | |
---|
| 157 | static void |
---|
| 158 | MultiplyAdd0_p81 (dReal * A, dReal * B, dReal * C, int p) |
---|
| 159 | { |
---|
| 160 | int i; |
---|
| 161 | dIASSERT (p > 0 && A && B && C); |
---|
| 162 | dReal sum; |
---|
| 163 | for (i = p; i; i--) |
---|
| 164 | { |
---|
| 165 | sum = B[0] * C[0]; |
---|
| 166 | sum += B[1] * C[1]; |
---|
| 167 | sum += B[2] * C[2]; |
---|
| 168 | sum += B[4] * C[4]; |
---|
| 169 | sum += B[5] * C[5]; |
---|
| 170 | sum += B[6] * C[6]; |
---|
| 171 | *(A++) += sum; |
---|
| 172 | B += 8; |
---|
| 173 | } |
---|
| 174 | } |
---|
| 175 | |
---|
| 176 | |
---|
| 177 | // this assumes the 4th and 8th rows of B are zero. |
---|
| 178 | |
---|
| 179 | static void |
---|
| 180 | Multiply1_8q1 (dReal * A, dReal * B, dReal * C, int q) |
---|
| 181 | { |
---|
| 182 | int k; |
---|
| 183 | dReal sum; |
---|
| 184 | dIASSERT (q > 0 && A && B && C); |
---|
| 185 | sum = 0; |
---|
| 186 | for (k = 0; k < q; k++) |
---|
| 187 | sum += B[k * 8] * C[k]; |
---|
| 188 | A[0] = sum; |
---|
| 189 | sum = 0; |
---|
| 190 | for (k = 0; k < q; k++) |
---|
| 191 | sum += B[1 + k * 8] * C[k]; |
---|
| 192 | A[1] = sum; |
---|
| 193 | sum = 0; |
---|
| 194 | for (k = 0; k < q; k++) |
---|
| 195 | sum += B[2 + k * 8] * C[k]; |
---|
| 196 | A[2] = sum; |
---|
| 197 | sum = 0; |
---|
| 198 | for (k = 0; k < q; k++) |
---|
| 199 | sum += B[4 + k * 8] * C[k]; |
---|
| 200 | A[4] = sum; |
---|
| 201 | sum = 0; |
---|
| 202 | for (k = 0; k < q; k++) |
---|
| 203 | sum += B[5 + k * 8] * C[k]; |
---|
| 204 | A[5] = sum; |
---|
| 205 | sum = 0; |
---|
| 206 | for (k = 0; k < q; k++) |
---|
| 207 | sum += B[6 + k * 8] * C[k]; |
---|
| 208 | A[6] = sum; |
---|
| 209 | } |
---|
| 210 | |
---|
| 211 | //**************************************************************************** |
---|
| 212 | // body rotation |
---|
| 213 | |
---|
| 214 | // return sin(x)/x. this has a singularity at 0 so special handling is needed |
---|
| 215 | // for small arguments. |
---|
| 216 | |
---|
| 217 | static inline dReal |
---|
| 218 | sinc (dReal x) |
---|
| 219 | { |
---|
| 220 | // if |x| < 1e-4 then use a taylor series expansion. this two term expansion |
---|
| 221 | // is actually accurate to one LS bit within this range if double precision |
---|
| 222 | // is being used - so don't worry! |
---|
| 223 | if (dFabs (x) < 1.0e-4) |
---|
| 224 | return REAL (1.0) - x * x * REAL (0.166666666666666666667); |
---|
| 225 | else |
---|
| 226 | return dSin (x) / x; |
---|
| 227 | } |
---|
| 228 | |
---|
| 229 | |
---|
| 230 | // given a body b, apply its linear and angular rotation over the time |
---|
| 231 | // interval h, thereby adjusting its position and orientation. |
---|
| 232 | |
---|
| 233 | static inline void |
---|
| 234 | moveAndRotateBody (dxBody * b, dReal h) |
---|
| 235 | { |
---|
| 236 | int j; |
---|
| 237 | |
---|
| 238 | // handle linear velocity |
---|
| 239 | for (j = 0; j < 3; j++) |
---|
| 240 | b->pos[j] += h * b->lvel[j]; |
---|
| 241 | |
---|
| 242 | if (b->flags & dxBodyFlagFiniteRotation) |
---|
| 243 | { |
---|
| 244 | dVector3 irv; // infitesimal rotation vector |
---|
| 245 | dQuaternion q; // quaternion for finite rotation |
---|
| 246 | |
---|
| 247 | if (b->flags & dxBodyFlagFiniteRotationAxis) |
---|
| 248 | { |
---|
| 249 | // split the angular velocity vector into a component along the finite |
---|
| 250 | // rotation axis, and a component orthogonal to it. |
---|
| 251 | dVector3 frv, irv; // finite rotation vector |
---|
| 252 | dReal k = dDOT (b->finite_rot_axis, b->avel); |
---|
| 253 | frv[0] = b->finite_rot_axis[0] * k; |
---|
| 254 | frv[1] = b->finite_rot_axis[1] * k; |
---|
| 255 | frv[2] = b->finite_rot_axis[2] * k; |
---|
| 256 | irv[0] = b->avel[0] - frv[0]; |
---|
| 257 | irv[1] = b->avel[1] - frv[1]; |
---|
| 258 | irv[2] = b->avel[2] - frv[2]; |
---|
| 259 | |
---|
| 260 | // make a rotation quaternion q that corresponds to frv * h. |
---|
| 261 | // compare this with the full-finite-rotation case below. |
---|
| 262 | h *= REAL (0.5); |
---|
| 263 | dReal theta = k * h; |
---|
| 264 | q[0] = dCos (theta); |
---|
| 265 | dReal s = sinc (theta) * h; |
---|
| 266 | q[1] = frv[0] * s; |
---|
| 267 | q[2] = frv[1] * s; |
---|
| 268 | q[3] = frv[2] * s; |
---|
| 269 | } |
---|
| 270 | else |
---|
| 271 | { |
---|
| 272 | // make a rotation quaternion q that corresponds to w * h |
---|
| 273 | dReal wlen = dSqrt (b->avel[0] * b->avel[0] + b->avel[1] * b->avel[1] + b->avel[2] * b->avel[2]); |
---|
| 274 | h *= REAL (0.5); |
---|
| 275 | dReal theta = wlen * h; |
---|
| 276 | q[0] = dCos (theta); |
---|
| 277 | dReal s = sinc (theta) * h; |
---|
| 278 | q[1] = b->avel[0] * s; |
---|
| 279 | q[2] = b->avel[1] * s; |
---|
| 280 | q[3] = b->avel[2] * s; |
---|
| 281 | } |
---|
| 282 | |
---|
| 283 | // do the finite rotation |
---|
| 284 | dQuaternion q2; |
---|
| 285 | dQMultiply0 (q2, q, b->q); |
---|
| 286 | for (j = 0; j < 4; j++) |
---|
| 287 | b->q[j] = q2[j]; |
---|
| 288 | |
---|
| 289 | // do the infitesimal rotation if required |
---|
| 290 | if (b->flags & dxBodyFlagFiniteRotationAxis) |
---|
| 291 | { |
---|
| 292 | dReal dq[4]; |
---|
| 293 | dWtoDQ (irv, b->q, dq); |
---|
| 294 | for (j = 0; j < 4; j++) |
---|
| 295 | b->q[j] += h * dq[j]; |
---|
| 296 | } |
---|
| 297 | } |
---|
| 298 | else |
---|
| 299 | { |
---|
| 300 | // the normal way - do an infitesimal rotation |
---|
| 301 | dReal dq[4]; |
---|
| 302 | dWtoDQ (b->avel, b->q, dq); |
---|
| 303 | for (j = 0; j < 4; j++) |
---|
| 304 | b->q[j] += h * dq[j]; |
---|
| 305 | } |
---|
| 306 | |
---|
| 307 | // normalize the quaternion and convert it to a rotation matrix |
---|
| 308 | dNormalize4 (b->q); |
---|
| 309 | dQtoR (b->q, b->R); |
---|
| 310 | |
---|
| 311 | // notify all attached geoms that this body has moved |
---|
| 312 | for (dxGeom * geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) |
---|
| 313 | dGeomMoved (geom); |
---|
| 314 | } |
---|
| 315 | |
---|
| 316 | //**************************************************************************** |
---|
| 317 | //This is an implementation of the iterated/relaxation algorithm. |
---|
| 318 | //Here is a quick overview of the algorithm per Sergi Valverde's posts to the |
---|
| 319 | //mailing list: |
---|
| 320 | // |
---|
| 321 | // for i=0..N-1 do |
---|
| 322 | // for c = 0..C-1 do |
---|
| 323 | // Solve constraint c-th |
---|
| 324 | // Apply forces to constraint bodies |
---|
| 325 | // next |
---|
| 326 | // next |
---|
| 327 | // Integrate bodies |
---|
| 328 | |
---|
| 329 | void |
---|
| 330 | dInternalStepFast (dxWorld * world, dxBody * body[2], dReal * GI[2], dReal * GinvI[2], dxJoint * joint, dxJoint::Info1 info, dxJoint::Info2 Jinfo, dReal stepsize) |
---|
| 331 | { |
---|
| 332 | int i, j, k; |
---|
| 333 | # ifdef TIMING |
---|
| 334 | dTimerNow ("constraint preprocessing"); |
---|
| 335 | # endif |
---|
| 336 | |
---|
| 337 | dReal stepsize1 = dRecip (stepsize); |
---|
| 338 | |
---|
| 339 | int m = info.m; |
---|
| 340 | // nothing to do if no constraints. |
---|
| 341 | if (m <= 0) |
---|
| 342 | return; |
---|
| 343 | |
---|
| 344 | int nub = 0; |
---|
| 345 | if (info.nub == info.m) |
---|
| 346 | nub = m; |
---|
| 347 | |
---|
| 348 | // compute A = J*invM*J'. first compute JinvM = J*invM. this has the same |
---|
| 349 | // format as J so we just go through the constraints in J multiplying by |
---|
| 350 | // the appropriate scalars and matrices. |
---|
| 351 | # ifdef TIMING |
---|
| 352 | dTimerNow ("compute A"); |
---|
| 353 | # endif |
---|
| 354 | dReal JinvM[2 * 6 * 8]; |
---|
| 355 | //dSetZero (JinvM, 2 * m * 8); |
---|
| 356 | |
---|
| 357 | dReal *Jsrc = Jinfo.J1l; |
---|
| 358 | dReal *Jdst = JinvM; |
---|
| 359 | if (body[0]) |
---|
| 360 | { |
---|
| 361 | for (j = m - 1; j >= 0; j--) |
---|
| 362 | { |
---|
| 363 | for (k = 0; k < 3; k++) |
---|
| 364 | Jdst[k] = Jsrc[k] * body[0]->invMass; |
---|
| 365 | dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[0]); |
---|
| 366 | Jsrc += 8; |
---|
| 367 | Jdst += 8; |
---|
| 368 | } |
---|
| 369 | } |
---|
| 370 | if (body[1]) |
---|
| 371 | { |
---|
| 372 | Jsrc = Jinfo.J2l; |
---|
| 373 | Jdst = JinvM + 8 * m; |
---|
| 374 | for (j = m - 1; j >= 0; j--) |
---|
| 375 | { |
---|
| 376 | for (k = 0; k < 3; k++) |
---|
| 377 | Jdst[k] = Jsrc[k] * body[1]->invMass; |
---|
| 378 | dMULTIPLY0_133 (Jdst + 4, Jsrc + 4, GinvI[1]); |
---|
| 379 | Jsrc += 8; |
---|
| 380 | Jdst += 8; |
---|
| 381 | } |
---|
| 382 | } |
---|
| 383 | |
---|
| 384 | |
---|
| 385 | // now compute A = JinvM * J'. |
---|
| 386 | int mskip = dPAD (m); |
---|
| 387 | dReal A[6 * 8]; |
---|
| 388 | //dSetZero (A, 6 * 8); |
---|
| 389 | |
---|
| 390 | if (body[0]) |
---|
| 391 | Multiply2_sym_p8p (A, JinvM, Jinfo.J1l, m, mskip); |
---|
| 392 | if (body[1]) |
---|
| 393 | MultiplyAdd2_sym_p8p (A, JinvM + 8 * m, Jinfo.J2l, m, mskip); |
---|
| 394 | |
---|
| 395 | // add cfm to the diagonal of A |
---|
| 396 | for (i = 0; i < m; i++) |
---|
| 397 | A[i * mskip + i] += Jinfo.cfm[i] * stepsize1; |
---|
| 398 | |
---|
| 399 | // compute the right hand side `rhs' |
---|
| 400 | # ifdef TIMING |
---|
| 401 | dTimerNow ("compute rhs"); |
---|
| 402 | # endif |
---|
| 403 | dReal tmp1[16]; |
---|
| 404 | //dSetZero (tmp1, 16); |
---|
| 405 | // put v/h + invM*fe into tmp1 |
---|
| 406 | for (i = 0; i < 2; i++) |
---|
| 407 | { |
---|
| 408 | if (!body[i]) |
---|
| 409 | continue; |
---|
| 410 | for (j = 0; j < 3; j++) |
---|
| 411 | tmp1[i * 8 + j] = body[i]->facc[j] * body[i]->invMass + body[i]->lvel[j] * stepsize1; |
---|
| 412 | dMULTIPLY0_331 (tmp1 + i * 8 + 4, GinvI[i], body[i]->tacc); |
---|
| 413 | for (j = 0; j < 3; j++) |
---|
| 414 | tmp1[i * 8 + 4 + j] += body[i]->avel[j] * stepsize1; |
---|
| 415 | } |
---|
| 416 | // put J*tmp1 into rhs |
---|
| 417 | dReal rhs[6]; |
---|
| 418 | //dSetZero (rhs, 6); |
---|
| 419 | |
---|
| 420 | if (body[0]) |
---|
| 421 | Multiply0_p81 (rhs, Jinfo.J1l, tmp1, m); |
---|
| 422 | if (body[1]) |
---|
| 423 | MultiplyAdd0_p81 (rhs, Jinfo.J2l, tmp1 + 8, m); |
---|
| 424 | |
---|
| 425 | // complete rhs |
---|
| 426 | for (i = 0; i < m; i++) |
---|
| 427 | rhs[i] = Jinfo.c[i] * stepsize1 - rhs[i]; |
---|
| 428 | |
---|
| 429 | #ifdef SLOW_LCP |
---|
| 430 | // solve the LCP problem and get lambda. |
---|
| 431 | // this will destroy A but that's okay |
---|
| 432 | # ifdef TIMING |
---|
| 433 | dTimerNow ("solving LCP problem"); |
---|
| 434 | # endif |
---|
| 435 | dReal *lambda = (dReal *) ALLOCA (m * sizeof (dReal)); |
---|
| 436 | dReal *residual = (dReal *) ALLOCA (m * sizeof (dReal)); |
---|
| 437 | dReal lo[6], hi[6]; |
---|
| 438 | memcpy (lo, Jinfo.lo, m * sizeof (dReal)); |
---|
| 439 | memcpy (hi, Jinfo.hi, m * sizeof (dReal)); |
---|
| 440 | dSolveLCP (m, A, lambda, rhs, residual, nub, lo, hi, Jinfo.findex); |
---|
| 441 | #endif |
---|
| 442 | |
---|
| 443 | // LCP Solver replacement: |
---|
| 444 | // This algorithm goes like this: |
---|
| 445 | // Do a straightforward LDLT factorization of the matrix A, solving for |
---|
| 446 | // A*x = rhs |
---|
| 447 | // For each x[i] that is outside of the bounds of lo[i] and hi[i], |
---|
| 448 | // clamp x[i] into that range. |
---|
| 449 | // Substitute into A the now known x's |
---|
| 450 | // subtract the residual away from the rhs. |
---|
| 451 | // Remove row and column i from L, updating the factorization |
---|
| 452 | // place the known x's at the end of the array, keeping up with location in p |
---|
| 453 | // Repeat until all constraints have been clamped or all are within bounds |
---|
| 454 | // |
---|
| 455 | // This is probably only faster in the single joint case where only one repeat is |
---|
| 456 | // the norm. |
---|
| 457 | |
---|
| 458 | #ifdef FAST_FACTOR |
---|
| 459 | // factorize A (L*D*L'=A) |
---|
| 460 | # ifdef TIMING |
---|
| 461 | dTimerNow ("factorize A"); |
---|
| 462 | # endif |
---|
| 463 | dReal d[6]; |
---|
| 464 | dReal L[6 * 8]; |
---|
| 465 | memcpy (L, A, m * mskip * sizeof (dReal)); |
---|
| 466 | dFactorLDLT (L, d, m, mskip); |
---|
| 467 | |
---|
| 468 | // compute lambda |
---|
| 469 | # ifdef TIMING |
---|
| 470 | dTimerNow ("compute lambda"); |
---|
| 471 | # endif |
---|
| 472 | |
---|
| 473 | int left = m; //constraints left to solve. |
---|
| 474 | int remove[6]; |
---|
| 475 | dReal lambda[6]; |
---|
| 476 | dReal x[6]; |
---|
| 477 | int p[6]; |
---|
| 478 | for (i = 0; i < 6; i++) |
---|
| 479 | p[i] = i; |
---|
| 480 | while (true) |
---|
| 481 | { |
---|
| 482 | memcpy (x, rhs, left * sizeof (dReal)); |
---|
| 483 | dSolveLDLT (L, d, x, left, mskip); |
---|
| 484 | |
---|
| 485 | int fixed = 0; |
---|
| 486 | for (i = 0; i < left; i++) |
---|
| 487 | { |
---|
| 488 | j = p[i]; |
---|
| 489 | remove[i] = false; |
---|
| 490 | // This isn't the exact same use of findex as dSolveLCP.... since x[findex] |
---|
| 491 | // may change after I've already clamped x[i], but it should be close |
---|
| 492 | if (Jinfo.findex[j] > -1) |
---|
| 493 | { |
---|
| 494 | dReal f = fabs (Jinfo.hi[j] * x[p[Jinfo.findex[j]]]); |
---|
| 495 | if (x[i] > f) |
---|
| 496 | x[i] = f; |
---|
| 497 | else if (x[i] < -f) |
---|
| 498 | x[i] = -f; |
---|
| 499 | else |
---|
| 500 | continue; |
---|
| 501 | } |
---|
| 502 | else |
---|
| 503 | { |
---|
| 504 | if (x[i] > Jinfo.hi[j]) |
---|
| 505 | x[i] = Jinfo.hi[j]; |
---|
| 506 | else if (x[i] < Jinfo.lo[j]) |
---|
| 507 | x[i] = Jinfo.lo[j]; |
---|
| 508 | else |
---|
| 509 | continue; |
---|
| 510 | } |
---|
| 511 | remove[i] = true; |
---|
| 512 | fixed++; |
---|
| 513 | } |
---|
| 514 | if (fixed == 0 || fixed == left) //no change or all constraints solved |
---|
| 515 | break; |
---|
| 516 | |
---|
| 517 | for (i = 0; i < left; i++) //sub in to right hand side. |
---|
| 518 | if (remove[i]) |
---|
| 519 | for (j = 0; j < left; j++) |
---|
| 520 | if (!remove[j]) |
---|
| 521 | rhs[j] -= A[j * mskip + i] * x[i]; |
---|
| 522 | |
---|
| 523 | for (int r = left - 1; r >= 0; r--) //eliminate row/col for fixed variables |
---|
| 524 | { |
---|
| 525 | if (remove[r]) |
---|
| 526 | { |
---|
| 527 | //dRemoveLDLT adapted for use without row pointers. |
---|
| 528 | if (r == left - 1) |
---|
| 529 | { |
---|
| 530 | left--; |
---|
| 531 | continue; // deleting last row/col is easy |
---|
| 532 | } |
---|
| 533 | else if (r == 0) |
---|
| 534 | { |
---|
| 535 | dReal a[6]; |
---|
| 536 | for (i = 0; i < left; i++) |
---|
| 537 | a[i] = -A[i * mskip]; |
---|
| 538 | a[0] += REAL (1.0); |
---|
| 539 | dLDLTAddTL (L, d, a, left, mskip); |
---|
| 540 | } |
---|
| 541 | else |
---|
| 542 | { |
---|
| 543 | dReal t[6]; |
---|
| 544 | dReal a[6]; |
---|
| 545 | for (i = 0; i < r; i++) |
---|
| 546 | t[i] = L[r * mskip + i] / d[i]; |
---|
| 547 | for (i = 0; i < left - r; i++) |
---|
| 548 | a[i] = dDot (L + (r + i) * mskip, t, r) - A[(r + i) * mskip + r]; |
---|
| 549 | a[0] += REAL (1.0); |
---|
| 550 | dLDLTAddTL (L + r * mskip + r, d + r, a, left - r, mskip); |
---|
| 551 | } |
---|
| 552 | |
---|
| 553 | dRemoveRowCol (L, left, mskip, r); |
---|
| 554 | //end dRemoveLDLT |
---|
| 555 | |
---|
| 556 | left--; |
---|
| 557 | if (r < (left - 1)) |
---|
| 558 | { |
---|
| 559 | dReal tx = x[r]; |
---|
| 560 | memmove (d + r, d + r + 1, (left - r) * sizeof (dReal)); |
---|
| 561 | memmove (rhs + r, rhs + r + 1, (left - r) * sizeof (dReal)); |
---|
| 562 | //x will get written over by rhs anyway, no need to move it around |
---|
| 563 | //just store the fixed value we just discovered in it. |
---|
| 564 | x[left] = tx; |
---|
| 565 | for (i = 0; i < m; i++) |
---|
| 566 | if (p[i] > r && p[i] <= left) |
---|
| 567 | p[i]--; |
---|
| 568 | p[r] = left; |
---|
| 569 | } |
---|
| 570 | } |
---|
| 571 | } |
---|
| 572 | } |
---|
| 573 | |
---|
| 574 | for (i = 0; i < m; i++) |
---|
| 575 | lambda[i] = x[p[i]]; |
---|
| 576 | # endif |
---|
| 577 | // compute the constraint force `cforce' |
---|
| 578 | # ifdef TIMING |
---|
| 579 | dTimerNow ("compute constraint force"); |
---|
| 580 | #endif |
---|
| 581 | |
---|
| 582 | // compute cforce = J'*lambda |
---|
| 583 | dJointFeedback *fb = joint->feedback; |
---|
| 584 | dReal cforce[16]; |
---|
| 585 | //dSetZero (cforce, 16); |
---|
| 586 | |
---|
| 587 | /******************** breakable joint contribution ***********************/ |
---|
| 588 | // this saves us a few dereferences |
---|
| 589 | dxJointBreakInfo *jBI = joint->breakInfo; |
---|
| 590 | // we need joint feedback if the joint is breakable or if the user |
---|
| 591 | // requested feedback. |
---|
| 592 | if (jBI||fb) { |
---|
| 593 | // we need feedback on the amount of force that this joint is |
---|
| 594 | // applying to the bodies. we use a slightly slower computation |
---|
| 595 | // that splits out the force components and puts them in the |
---|
| 596 | // feedback structure. |
---|
| 597 | dJointFeedback temp_fb; // temporary storage for joint feedback |
---|
| 598 | dReal data1[8],data2[8]; |
---|
| 599 | if (body[0]) |
---|
| 600 | { |
---|
| 601 | Multiply1_8q1 (data1, Jinfo.J1l, lambda, m); |
---|
| 602 | dReal *cf1 = cforce; |
---|
| 603 | cf1[0] = (temp_fb.f1[0] = data1[0]); |
---|
| 604 | cf1[1] = (temp_fb.f1[1] = data1[1]); |
---|
| 605 | cf1[2] = (temp_fb.f1[2] = data1[2]); |
---|
| 606 | cf1[4] = (temp_fb.t1[0] = data1[4]); |
---|
| 607 | cf1[5] = (temp_fb.t1[1] = data1[5]); |
---|
| 608 | cf1[6] = (temp_fb.t1[2] = data1[6]); |
---|
| 609 | } |
---|
| 610 | if (body[1]) |
---|
| 611 | { |
---|
| 612 | Multiply1_8q1 (data2, Jinfo.J2l, lambda, m); |
---|
| 613 | dReal *cf2 = cforce + 8; |
---|
| 614 | cf2[0] = (temp_fb.f2[0] = data2[0]); |
---|
| 615 | cf2[1] = (temp_fb.f2[1] = data2[1]); |
---|
| 616 | cf2[2] = (temp_fb.f2[2] = data2[2]); |
---|
| 617 | cf2[4] = (temp_fb.t2[0] = data2[4]); |
---|
| 618 | cf2[5] = (temp_fb.t2[1] = data2[5]); |
---|
| 619 | cf2[6] = (temp_fb.t2[2] = data2[6]); |
---|
| 620 | } |
---|
| 621 | // if the user requested so we must copy the feedback information to |
---|
| 622 | // the feedback struct that the user suplied. |
---|
| 623 | if (fb) { |
---|
| 624 | // copy temp_fb to fb |
---|
| 625 | fb->f1[0] = temp_fb.f1[0]; |
---|
| 626 | fb->f1[1] = temp_fb.f1[1]; |
---|
| 627 | fb->f1[2] = temp_fb.f1[2]; |
---|
| 628 | fb->t1[0] = temp_fb.t1[0]; |
---|
| 629 | fb->t1[1] = temp_fb.t1[1]; |
---|
| 630 | fb->t1[2] = temp_fb.t1[2]; |
---|
| 631 | if (body[1]) { |
---|
| 632 | fb->f2[0] = temp_fb.f2[0]; |
---|
| 633 | fb->f2[1] = temp_fb.f2[1]; |
---|
| 634 | fb->f2[2] = temp_fb.f2[2]; |
---|
| 635 | fb->t2[0] = temp_fb.t2[0]; |
---|
| 636 | fb->t2[1] = temp_fb.t2[1]; |
---|
| 637 | fb->t2[2] = temp_fb.t2[2]; |
---|
| 638 | } |
---|
| 639 | } |
---|
| 640 | // if the joint is breakable we need to check the breaking conditions |
---|
| 641 | if (jBI) { |
---|
| 642 | dReal relCF1[3]; |
---|
| 643 | dReal relCT1[3]; |
---|
| 644 | // multiply the force and torque vectors by the rotation matrix of body 1 |
---|
| 645 | dMULTIPLY1_331 (&relCF1[0],body[0]->R,&temp_fb.f1[0]); |
---|
| 646 | dMULTIPLY1_331 (&relCT1[0],body[0]->R,&temp_fb.t1[0]); |
---|
| 647 | if (jBI->flags & dJOINT_BREAK_AT_B1_FORCE) { |
---|
| 648 | // check if the force is to high |
---|
| 649 | for (int i = 0; i < 3; i++) { |
---|
| 650 | if (relCF1[i] > jBI->b1MaxF[i]) { |
---|
| 651 | jBI->flags |= dJOINT_BROKEN; |
---|
| 652 | goto doneCheckingBreaks; |
---|
| 653 | } |
---|
| 654 | } |
---|
| 655 | } |
---|
| 656 | if (jBI->flags & dJOINT_BREAK_AT_B1_TORQUE) { |
---|
| 657 | // check if the torque is to high |
---|
| 658 | for (int i = 0; i < 3; i++) { |
---|
| 659 | if (relCT1[i] > jBI->b1MaxT[i]) { |
---|
| 660 | jBI->flags |= dJOINT_BROKEN; |
---|
| 661 | goto doneCheckingBreaks; |
---|
| 662 | } |
---|
| 663 | } |
---|
| 664 | } |
---|
| 665 | if (body[1]) { |
---|
| 666 | dReal relCF2[3]; |
---|
| 667 | dReal relCT2[3]; |
---|
| 668 | // multiply the force and torque vectors by the rotation matrix of body 2 |
---|
| 669 | dMULTIPLY1_331 (&relCF2[0],body[1]->R,&temp_fb.f2[0]); |
---|
| 670 | dMULTIPLY1_331 (&relCT2[0],body[1]->R,&temp_fb.t2[0]); |
---|
| 671 | if (jBI->flags & dJOINT_BREAK_AT_B2_FORCE) { |
---|
| 672 | // check if the force is to high |
---|
| 673 | for (int i = 0; i < 3; i++) { |
---|
| 674 | if (relCF2[i] > jBI->b2MaxF[i]) { |
---|
| 675 | jBI->flags |= dJOINT_BROKEN; |
---|
| 676 | goto doneCheckingBreaks; |
---|
| 677 | } |
---|
| 678 | } |
---|
| 679 | } |
---|
| 680 | if (jBI->flags & dJOINT_BREAK_AT_B2_TORQUE) { |
---|
| 681 | // check if the torque is to high |
---|
| 682 | for (int i = 0; i < 3; i++) { |
---|
| 683 | if (relCT2[i] > jBI->b2MaxT[i]) { |
---|
| 684 | jBI->flags |= dJOINT_BROKEN; |
---|
| 685 | goto doneCheckingBreaks; |
---|
| 686 | } |
---|
| 687 | } |
---|
| 688 | } |
---|
| 689 | } |
---|
| 690 | doneCheckingBreaks: |
---|
| 691 | ; |
---|
| 692 | } |
---|
| 693 | } |
---|
| 694 | /*************************************************************************/ |
---|
| 695 | else |
---|
| 696 | { |
---|
| 697 | // no feedback is required, let's compute cforce the faster way |
---|
| 698 | if (body[0]) |
---|
| 699 | Multiply1_8q1 (cforce, Jinfo.J1l, lambda, m); |
---|
| 700 | if (body[1]) |
---|
| 701 | Multiply1_8q1 (cforce + 8, Jinfo.J2l, lambda, m); |
---|
| 702 | } |
---|
| 703 | |
---|
| 704 | for (i = 0; i < 2; i++) |
---|
| 705 | { |
---|
| 706 | if (!body[i]) |
---|
| 707 | continue; |
---|
| 708 | for (j = 0; j < 3; j++) |
---|
| 709 | { |
---|
| 710 | body[i]->facc[j] += cforce[i * 8 + j]; |
---|
| 711 | body[i]->tacc[j] += cforce[i * 8 + 4 + j]; |
---|
| 712 | } |
---|
| 713 | } |
---|
| 714 | } |
---|
| 715 | |
---|
| 716 | void |
---|
| 717 | dInternalStepIslandFast (dxWorld * world, dxBody * const *bodies, int nb, dxJoint * const *_joints, int nj, dReal stepsize, int maxiterations) |
---|
| 718 | { |
---|
| 719 | # ifdef TIMING |
---|
| 720 | dTimerNow ("preprocessing"); |
---|
| 721 | # endif |
---|
| 722 | dxBody *bodyPair[2], *body; |
---|
| 723 | dReal *GIPair[2], *GinvIPair[2]; |
---|
| 724 | dxJoint *joint; |
---|
| 725 | int iter, b, j, i; |
---|
| 726 | dReal ministep = stepsize / maxiterations; |
---|
| 727 | |
---|
| 728 | // make a local copy of the joint array, because we might want to modify it. |
---|
| 729 | // (the "dxJoint *const*" declaration says we're allowed to modify the joints |
---|
| 730 | // but not the joint array, because the caller might need it unchanged). |
---|
| 731 | dxJoint **joints = (dxJoint **) ALLOCA (nj * sizeof (dxJoint *)); |
---|
| 732 | memcpy (joints, _joints, nj * sizeof (dxJoint *)); |
---|
| 733 | |
---|
| 734 | // get m = total constraint dimension, nub = number of unbounded variables. |
---|
| 735 | // create constraint offset array and number-of-rows array for all joints. |
---|
| 736 | // the constraints are re-ordered as follows: the purely unbounded |
---|
| 737 | // constraints, the mixed unbounded + LCP constraints, and last the purely |
---|
| 738 | // LCP constraints. this assists the LCP solver to put all unbounded |
---|
| 739 | // variables at the start for a quick factorization. |
---|
| 740 | // |
---|
| 741 | // joints with m=0 are inactive and are removed from the joints array |
---|
| 742 | // entirely, so that the code that follows does not consider them. |
---|
| 743 | // also number all active joints in the joint list (set their tag values). |
---|
| 744 | // inactive joints receive a tag value of -1. |
---|
| 745 | |
---|
| 746 | int m = 0; |
---|
| 747 | dxJoint::Info1 * info = (dxJoint::Info1 *) ALLOCA (nj * sizeof (dxJoint::Info1)); |
---|
| 748 | int *ofs = (int *) ALLOCA (nj * sizeof (int)); |
---|
| 749 | for (i = 0, j = 0; j < nj; j++) |
---|
| 750 | { // i=dest, j=src |
---|
| 751 | joints[j]->vtable->getInfo1 (joints[j], info + i); |
---|
| 752 | dIASSERT (info[i].m >= 0 && info[i].m <= 6 && info[i].nub >= 0 && info[i].nub <= info[i].m); |
---|
| 753 | if (info[i].m > 0) |
---|
| 754 | { |
---|
| 755 | joints[i] = joints[j]; |
---|
| 756 | joints[i]->tag = i; |
---|
| 757 | i++; |
---|
| 758 | } |
---|
| 759 | else |
---|
| 760 | { |
---|
| 761 | joints[j]->tag = -1; |
---|
| 762 | } |
---|
| 763 | } |
---|
| 764 | nj = i; |
---|
| 765 | |
---|
| 766 | // the purely unbounded constraints |
---|
| 767 | for (i = 0; i < nj; i++) |
---|
| 768 | { |
---|
| 769 | ofs[i] = m; |
---|
| 770 | m += info[i].m; |
---|
| 771 | } |
---|
| 772 | dReal *c = NULL; |
---|
| 773 | dReal *cfm = NULL; |
---|
| 774 | dReal *lo = NULL; |
---|
| 775 | dReal *hi = NULL; |
---|
| 776 | int *findex = NULL; |
---|
| 777 | |
---|
| 778 | dReal *J = NULL; |
---|
| 779 | dxJoint::Info2 * Jinfo = NULL; |
---|
| 780 | |
---|
| 781 | if (m) |
---|
| 782 | { |
---|
| 783 | // create a constraint equation right hand side vector `c', a constraint |
---|
| 784 | // force mixing vector `cfm', and LCP low and high bound vectors, and an |
---|
| 785 | // 'findex' vector. |
---|
| 786 | c = (dReal *) ALLOCA (m * sizeof (dReal)); |
---|
| 787 | cfm = (dReal *) ALLOCA (m * sizeof (dReal)); |
---|
| 788 | lo = (dReal *) ALLOCA (m * sizeof (dReal)); |
---|
| 789 | hi = (dReal *) ALLOCA (m * sizeof (dReal)); |
---|
| 790 | findex = (int *) ALLOCA (m * sizeof (int)); |
---|
| 791 | dSetZero (c, m); |
---|
| 792 | dSetValue (cfm, m, world->global_cfm); |
---|
| 793 | dSetValue (lo, m, -dInfinity); |
---|
| 794 | dSetValue (hi, m, dInfinity); |
---|
| 795 | for (i = 0; i < m; i++) |
---|
| 796 | findex[i] = -1; |
---|
| 797 | |
---|
| 798 | // get jacobian data from constraints. a (2*m)x8 matrix will be created |
---|
| 799 | // to store the two jacobian blocks from each constraint. it has this |
---|
| 800 | // format: |
---|
| 801 | // |
---|
| 802 | // l l l 0 a a a 0 \ . |
---|
| 803 | // l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 rows) |
---|
| 804 | // l l l 0 a a a 0 / |
---|
| 805 | // l l l 0 a a a 0 \ . |
---|
| 806 | // l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 rows) |
---|
| 807 | // l l l 0 a a a 0 / |
---|
| 808 | // l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row) |
---|
| 809 | // l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row) |
---|
| 810 | // etc... |
---|
| 811 | // |
---|
| 812 | // (lll) = linear jacobian data |
---|
| 813 | // (aaa) = angular jacobian data |
---|
| 814 | // |
---|
| 815 | # ifdef TIMING |
---|
| 816 | dTimerNow ("create J"); |
---|
| 817 | # endif |
---|
| 818 | J = (dReal *) ALLOCA (2 * m * 8 * sizeof (dReal)); |
---|
| 819 | dSetZero (J, 2 * m * 8); |
---|
| 820 | Jinfo = (dxJoint::Info2 *) ALLOCA (nj * sizeof (dxJoint::Info2)); |
---|
| 821 | for (i = 0; i < nj; i++) |
---|
| 822 | { |
---|
| 823 | Jinfo[i].rowskip = 8; |
---|
| 824 | Jinfo[i].fps = dRecip (stepsize); |
---|
| 825 | Jinfo[i].erp = world->global_erp; |
---|
| 826 | Jinfo[i].J1l = J + 2 * 8 * ofs[i]; |
---|
| 827 | Jinfo[i].J1a = Jinfo[i].J1l + 4; |
---|
| 828 | Jinfo[i].J2l = Jinfo[i].J1l + 8 * info[i].m; |
---|
| 829 | Jinfo[i].J2a = Jinfo[i].J2l + 4; |
---|
| 830 | Jinfo[i].c = c + ofs[i]; |
---|
| 831 | Jinfo[i].cfm = cfm + ofs[i]; |
---|
| 832 | Jinfo[i].lo = lo + ofs[i]; |
---|
| 833 | Jinfo[i].hi = hi + ofs[i]; |
---|
| 834 | Jinfo[i].findex = findex + ofs[i]; |
---|
| 835 | //joints[i]->vtable->getInfo2 (joints[i], Jinfo+i); |
---|
| 836 | } |
---|
| 837 | |
---|
| 838 | } |
---|
| 839 | |
---|
| 840 | dReal *saveFacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal)); |
---|
| 841 | dReal *saveTacc = (dReal *) ALLOCA (nb * 4 * sizeof (dReal)); |
---|
| 842 | dReal *globalI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal)); |
---|
| 843 | dReal *globalInvI = (dReal *) ALLOCA (nb * 12 * sizeof (dReal)); |
---|
| 844 | for (b = 0; b < nb; b++) |
---|
| 845 | { |
---|
| 846 | for (i = 0; i < 4; i++) |
---|
| 847 | { |
---|
| 848 | saveFacc[b * 4 + i] = bodies[b]->facc[i]; |
---|
| 849 | saveTacc[b * 4 + i] = bodies[b]->tacc[i]; |
---|
| 850 | bodies[b]->tag = b; |
---|
| 851 | } |
---|
| 852 | } |
---|
| 853 | |
---|
| 854 | for (iter = 0; iter < maxiterations; iter++) |
---|
| 855 | { |
---|
| 856 | # ifdef TIMING |
---|
| 857 | dTimerNow ("applying inertia and gravity"); |
---|
| 858 | # endif |
---|
| 859 | dReal tmp[12] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; |
---|
| 860 | |
---|
| 861 | for (b = 0; b < nb; b++) |
---|
| 862 | { |
---|
| 863 | body = bodies[b]; |
---|
| 864 | |
---|
| 865 | // for all bodies, compute the inertia tensor and its inverse in the global |
---|
| 866 | // frame, and compute the rotational force and add it to the torque |
---|
| 867 | // accumulator. I and invI are vertically stacked 3x4 matrices, one per body. |
---|
| 868 | // @@@ check computation of rotational force. |
---|
| 869 | |
---|
| 870 | // compute inertia tensor in global frame |
---|
| 871 | dMULTIPLY2_333 (tmp, body->mass.I, body->R); |
---|
| 872 | dMULTIPLY0_333 (globalI + b * 12, body->R, tmp); |
---|
| 873 | // compute inverse inertia tensor in global frame |
---|
| 874 | dMULTIPLY2_333 (tmp, body->invI, body->R); |
---|
| 875 | dMULTIPLY0_333 (globalInvI + b * 12, body->R, tmp); |
---|
| 876 | |
---|
| 877 | for (i = 0; i < 4; i++) |
---|
| 878 | body->tacc[i] = saveTacc[b * 4 + i]; |
---|
| 879 | // compute rotational force |
---|
| 880 | dMULTIPLY0_331 (tmp, globalI + b * 12, body->avel); |
---|
| 881 | dCROSS (body->tacc, -=, body->avel, tmp); |
---|
| 882 | |
---|
| 883 | // add the gravity force to all bodies |
---|
| 884 | if ((body->flags & dxBodyNoGravity) == 0) |
---|
| 885 | { |
---|
| 886 | body->facc[0] = saveFacc[b * 4 + 0] + body->mass.mass * world->gravity[0]; |
---|
| 887 | body->facc[1] = saveFacc[b * 4 + 1] + body->mass.mass * world->gravity[1]; |
---|
| 888 | body->facc[2] = saveFacc[b * 4 + 2] + body->mass.mass * world->gravity[2]; |
---|
| 889 | body->facc[3] = 0; |
---|
| 890 | } |
---|
| 891 | |
---|
| 892 | } |
---|
| 893 | |
---|
| 894 | #ifdef RANDOM_JOINT_ORDER |
---|
| 895 | #ifdef TIMING |
---|
| 896 | dTimerNow ("randomizing joint order"); |
---|
| 897 | #endif |
---|
| 898 | //randomize the order of the joints by looping through the array |
---|
| 899 | //and swapping the current joint pointer with a random one before it. |
---|
| 900 | for (j = 0; j < nj; j++) |
---|
| 901 | { |
---|
| 902 | joint = joints[j]; |
---|
| 903 | dxJoint::Info1 i1 = info[j]; |
---|
| 904 | dxJoint::Info2 i2 = Jinfo[j]; |
---|
| 905 | int r = rand () % (j + 1); |
---|
| 906 | joints[j] = joints[r]; |
---|
| 907 | info[j] = info[r]; |
---|
| 908 | Jinfo[j] = Jinfo[r]; |
---|
| 909 | joints[r] = joint; |
---|
| 910 | info[r] = i1; |
---|
| 911 | Jinfo[r] = i2; |
---|
| 912 | } |
---|
| 913 | #endif |
---|
| 914 | |
---|
| 915 | //now iterate through the random ordered joint array we created. |
---|
| 916 | for (j = 0; j < nj; j++) |
---|
| 917 | { |
---|
| 918 | #ifdef TIMING |
---|
| 919 | dTimerNow ("setting up joint"); |
---|
| 920 | #endif |
---|
| 921 | joint = joints[j]; |
---|
| 922 | bodyPair[0] = joint->node[0].body; |
---|
| 923 | bodyPair[1] = joint->node[1].body; |
---|
| 924 | |
---|
| 925 | if (bodyPair[0] && (bodyPair[0]->flags & dxBodyDisabled)) |
---|
| 926 | bodyPair[0] = 0; |
---|
| 927 | if (bodyPair[1] && (bodyPair[1]->flags & dxBodyDisabled)) |
---|
| 928 | bodyPair[1] = 0; |
---|
| 929 | |
---|
| 930 | //if this joint is not connected to any enabled bodies, skip it. |
---|
| 931 | if (!bodyPair[0] && !bodyPair[1]) |
---|
| 932 | continue; |
---|
| 933 | |
---|
| 934 | if (bodyPair[0]) |
---|
| 935 | { |
---|
| 936 | GIPair[0] = globalI + bodyPair[0]->tag * 12; |
---|
| 937 | GinvIPair[0] = globalInvI + bodyPair[0]->tag * 12; |
---|
| 938 | } |
---|
| 939 | if (bodyPair[1]) |
---|
| 940 | { |
---|
| 941 | GIPair[1] = globalI + bodyPair[1]->tag * 12; |
---|
| 942 | GinvIPair[1] = globalInvI + bodyPair[1]->tag * 12; |
---|
| 943 | } |
---|
| 944 | |
---|
| 945 | joints[j]->vtable->getInfo2 (joints[j], Jinfo + j); |
---|
| 946 | |
---|
| 947 | //dInternalStepIslandFast is an exact copy of the old routine with one |
---|
| 948 | //modification: the calculated forces are added back to the facc and tacc |
---|
| 949 | //vectors instead of applying them to the bodies and moving them. |
---|
| 950 | if (info[j].m > 0) |
---|
| 951 | { |
---|
| 952 | dInternalStepFast (world, bodyPair, GIPair, GinvIPair, joint, info[j], Jinfo[j], ministep); |
---|
| 953 | } |
---|
| 954 | } |
---|
| 955 | // } |
---|
| 956 | # ifdef TIMING |
---|
| 957 | dTimerNow ("moving bodies"); |
---|
| 958 | # endif |
---|
| 959 | //Now we can simulate all the free floating bodies, and move them. |
---|
| 960 | for (b = 0; b < nb; b++) |
---|
| 961 | { |
---|
| 962 | body = bodies[b]; |
---|
| 963 | |
---|
| 964 | for (i = 0; i < 4; i++) |
---|
| 965 | { |
---|
| 966 | body->facc[i] *= ministep; |
---|
| 967 | body->tacc[i] *= ministep; |
---|
| 968 | } |
---|
| 969 | |
---|
| 970 | //apply torque |
---|
| 971 | dMULTIPLYADD0_331 (body->avel, globalInvI + b * 12, body->tacc); |
---|
| 972 | |
---|
| 973 | //apply force |
---|
| 974 | for (i = 0; i < 3; i++) |
---|
| 975 | body->lvel[i] += body->invMass * body->facc[i]; |
---|
| 976 | |
---|
| 977 | //move It! |
---|
| 978 | moveAndRotateBody (body, ministep); |
---|
| 979 | } |
---|
| 980 | } |
---|
| 981 | for (b = 0; b < nb; b++) |
---|
| 982 | for (j = 0; j < 4; j++) |
---|
| 983 | bodies[b]->facc[j] = bodies[b]->tacc[j] = 0; |
---|
| 984 | } |
---|
| 985 | |
---|
| 986 | |
---|
| 987 | #ifdef NO_ISLANDS |
---|
| 988 | |
---|
| 989 | // Since the iterative algorithm doesn't care about islands of bodies, this is a |
---|
| 990 | // faster algorithm that just sends it all the joints and bodies in one array. |
---|
| 991 | // It's downfall is it's inability to handle disabled bodies as well as the old one. |
---|
| 992 | static void |
---|
| 993 | processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations) |
---|
| 994 | { |
---|
| 995 | // nothing to do if no bodies |
---|
| 996 | if (world->nb <= 0) |
---|
| 997 | return; |
---|
| 998 | |
---|
| 999 | # ifdef TIMING |
---|
| 1000 | dTimerStart ("creating joint and body arrays"); |
---|
| 1001 | # endif |
---|
| 1002 | dxBody **bodies, *body; |
---|
| 1003 | dxJoint **joints, *joint; |
---|
| 1004 | joints = (dxJoint **) ALLOCA (world->nj * sizeof (dxJoint *)); |
---|
| 1005 | bodies = (dxBody **) ALLOCA (world->nb * sizeof (dxBody *)); |
---|
| 1006 | |
---|
| 1007 | int nj = 0; |
---|
| 1008 | for (joint = world->firstjoint; joint; joint = (dxJoint *) joint->next) |
---|
| 1009 | joints[nj++] = joint; |
---|
| 1010 | |
---|
| 1011 | int nb = 0; |
---|
| 1012 | for (body = world->firstbody; body; body = (dxBody *) body->next) |
---|
| 1013 | bodies[nb++] = body; |
---|
| 1014 | |
---|
| 1015 | dInternalStepIslandFast (world, bodies, nb, joints, nj, stepsize, maxiterations); |
---|
| 1016 | # ifdef TIMING |
---|
| 1017 | dTimerEnd (); |
---|
| 1018 | dTimerReport (stdout, 1); |
---|
| 1019 | # endif |
---|
| 1020 | } |
---|
| 1021 | |
---|
| 1022 | #else |
---|
| 1023 | |
---|
| 1024 | //**************************************************************************** |
---|
| 1025 | // island processing |
---|
| 1026 | |
---|
| 1027 | // this groups all joints and bodies in a world into islands. all objects |
---|
| 1028 | // in an island are reachable by going through connected bodies and joints. |
---|
| 1029 | // each island can be simulated separately. |
---|
| 1030 | // note that joints that are not attached to anything will not be included |
---|
| 1031 | // in any island, an so they do not affect the simulation. |
---|
| 1032 | // |
---|
| 1033 | // this function starts new island from unvisited bodies. however, it will |
---|
| 1034 | // never start a new islands from a disabled body. thus islands of disabled |
---|
| 1035 | // bodies will not be included in the simulation. disabled bodies are |
---|
| 1036 | // re-enabled if they are found to be part of an active island. |
---|
| 1037 | |
---|
| 1038 | static void |
---|
| 1039 | processIslandsFast (dxWorld * world, dReal stepsize, int maxiterations) |
---|
| 1040 | { |
---|
| 1041 | #ifdef TIMING |
---|
| 1042 | dTimerStart ("Island Setup"); |
---|
| 1043 | #endif |
---|
| 1044 | dxBody *b, *bb, **body; |
---|
| 1045 | dxJoint *j, **joint; |
---|
| 1046 | |
---|
| 1047 | // nothing to do if no bodies |
---|
| 1048 | if (world->nb <= 0) |
---|
| 1049 | return; |
---|
| 1050 | |
---|
| 1051 | // make arrays for body and joint lists (for a single island) to go into |
---|
| 1052 | body = (dxBody **) ALLOCA (world->nb * sizeof (dxBody *)); |
---|
| 1053 | joint = (dxJoint **) ALLOCA (world->nj * sizeof (dxJoint *)); |
---|
| 1054 | int bcount = 0; // number of bodies in `body' |
---|
| 1055 | int jcount = 0; // number of joints in `joint' |
---|
| 1056 | int tbcount = 0; |
---|
| 1057 | int tjcount = 0; |
---|
| 1058 | |
---|
| 1059 | // set all body/joint tags to 0 |
---|
| 1060 | for (b = world->firstbody; b; b = (dxBody *) b->next) |
---|
| 1061 | b->tag = 0; |
---|
| 1062 | for (j = world->firstjoint; j; j = (dxJoint *) j->next) |
---|
| 1063 | j->tag = 0; |
---|
| 1064 | |
---|
| 1065 | // allocate a stack of unvisited bodies in the island. the maximum size of |
---|
| 1066 | // the stack can be the lesser of the number of bodies or joints, because |
---|
| 1067 | // new bodies are only ever added to the stack by going through untagged |
---|
| 1068 | // joints. all the bodies in the stack must be tagged! |
---|
| 1069 | int stackalloc = (world->nj < world->nb) ? world->nj : world->nb; |
---|
| 1070 | dxBody **stack = (dxBody **) ALLOCA (stackalloc * sizeof (dxBody *)); |
---|
| 1071 | int *autostack = (int *) ALLOCA (stackalloc * sizeof (int)); |
---|
| 1072 | |
---|
| 1073 | for (bb = world->firstbody; bb; bb = (dxBody *) bb->next) |
---|
| 1074 | { |
---|
| 1075 | #ifdef TIMING |
---|
| 1076 | dTimerNow ("Island Processing"); |
---|
| 1077 | #endif |
---|
| 1078 | // get bb = the next enabled, untagged body, and tag it |
---|
| 1079 | if (bb->tag || (bb->flags & dxBodyDisabled)) |
---|
| 1080 | continue; |
---|
| 1081 | bb->tag = 1; |
---|
| 1082 | |
---|
| 1083 | // tag all bodies and joints starting from bb. |
---|
| 1084 | int stacksize = 0; |
---|
| 1085 | int autoDepth = autoEnableDepth; |
---|
| 1086 | b = bb; |
---|
| 1087 | body[0] = bb; |
---|
| 1088 | bcount = 1; |
---|
| 1089 | jcount = 0; |
---|
| 1090 | goto quickstart; |
---|
| 1091 | while (stacksize > 0) |
---|
| 1092 | { |
---|
| 1093 | b = stack[--stacksize]; // pop body off stack |
---|
| 1094 | autoDepth = autostack[stacksize]; |
---|
| 1095 | body[bcount++] = b; // put body on body list |
---|
| 1096 | quickstart: |
---|
| 1097 | |
---|
| 1098 | // traverse and tag all body's joints, add untagged connected bodies |
---|
| 1099 | // to stack |
---|
| 1100 | for (dxJointNode * n = b->firstjoint; n; n = n->next) |
---|
| 1101 | { |
---|
| 1102 | if (!n->joint->tag) |
---|
| 1103 | { |
---|
| 1104 | int thisDepth = autoEnableDepth; |
---|
| 1105 | n->joint->tag = 1; |
---|
| 1106 | joint[jcount++] = n->joint; |
---|
| 1107 | if (n->body && !n->body->tag) |
---|
| 1108 | { |
---|
| 1109 | if (n->body->flags & dxBodyDisabled) |
---|
| 1110 | thisDepth = autoDepth - 1; |
---|
| 1111 | if (thisDepth < 0) |
---|
| 1112 | continue; |
---|
| 1113 | n->body->flags &= ~dxBodyDisabled; |
---|
| 1114 | n->body->tag = 1; |
---|
| 1115 | autostack[stacksize] = thisDepth; |
---|
| 1116 | stack[stacksize++] = n->body; |
---|
| 1117 | } |
---|
| 1118 | } |
---|
| 1119 | } |
---|
| 1120 | dIASSERT (stacksize <= world->nb); |
---|
| 1121 | dIASSERT (stacksize <= world->nj); |
---|
| 1122 | } |
---|
| 1123 | |
---|
| 1124 | // now do something with body and joint lists |
---|
| 1125 | dInternalStepIslandFast (world, body, bcount, joint, jcount, stepsize, maxiterations); |
---|
| 1126 | |
---|
| 1127 | // what we've just done may have altered the body/joint tag values. |
---|
| 1128 | // we must make sure that these tags are nonzero. |
---|
| 1129 | // also make sure all bodies are in the enabled state. |
---|
| 1130 | int i; |
---|
| 1131 | for (i = 0; i < bcount; i++) |
---|
| 1132 | { |
---|
| 1133 | body[i]->tag = 1; |
---|
| 1134 | body[i]->flags &= ~dxBodyDisabled; |
---|
| 1135 | } |
---|
| 1136 | for (i = 0; i < jcount; i++) |
---|
| 1137 | joint[i]->tag = 1; |
---|
| 1138 | |
---|
| 1139 | tbcount += bcount; |
---|
| 1140 | tjcount += jcount; |
---|
| 1141 | } |
---|
| 1142 | |
---|
| 1143 | #ifdef TIMING |
---|
| 1144 | dMessage(0, "Total joints processed: %i, bodies: %i", tjcount, tbcount); |
---|
| 1145 | #endif |
---|
| 1146 | |
---|
| 1147 | // if debugging, check that all objects (except for disabled bodies, |
---|
| 1148 | // unconnected joints, and joints that are connected to disabled bodies) |
---|
| 1149 | // were tagged. |
---|
| 1150 | # ifndef dNODEBUG |
---|
| 1151 | for (b = world->firstbody; b; b = (dxBody *) b->next) |
---|
| 1152 | { |
---|
| 1153 | if (b->flags & dxBodyDisabled) |
---|
| 1154 | { |
---|
| 1155 | if (b->tag) |
---|
| 1156 | dDebug (0, "disabled body tagged"); |
---|
| 1157 | } |
---|
| 1158 | else |
---|
| 1159 | { |
---|
| 1160 | if (!b->tag) |
---|
| 1161 | dDebug (0, "enabled body not tagged"); |
---|
| 1162 | } |
---|
| 1163 | } |
---|
| 1164 | for (j = world->firstjoint; j; j = (dxJoint *) j->next) |
---|
| 1165 | { |
---|
| 1166 | if ((j->node[0].body && (j->node[0].body->flags & dxBodyDisabled) == 0) || (j->node[1].body && (j->node[1].body->flags & dxBodyDisabled) == 0)) |
---|
| 1167 | { |
---|
| 1168 | if (!j->tag) |
---|
| 1169 | dDebug (0, "attached enabled joint not tagged"); |
---|
| 1170 | } |
---|
| 1171 | else |
---|
| 1172 | { |
---|
| 1173 | if (j->tag) |
---|
| 1174 | dDebug (0, "unattached or disabled joint tagged"); |
---|
| 1175 | } |
---|
| 1176 | } |
---|
| 1177 | # endif |
---|
| 1178 | /******************** breakable joint contribution ***********************/ |
---|
| 1179 | dxJoint* nextJ; |
---|
| 1180 | if (!world->firstjoint) |
---|
| 1181 | nextJ = 0; |
---|
| 1182 | else |
---|
| 1183 | nextJ = (dxJoint*)world->firstjoint->next; |
---|
| 1184 | for (j=world->firstjoint; j; j=nextJ) { |
---|
| 1185 | nextJ = (dxJoint*)j->next; |
---|
| 1186 | // check if joint is breakable and broken |
---|
| 1187 | if (j->breakInfo && j->breakInfo->flags & dJOINT_BROKEN) { |
---|
| 1188 | // detach (break) the joint |
---|
| 1189 | dJointAttach (j, 0, 0); |
---|
| 1190 | // call the callback function if it is set |
---|
| 1191 | if (j->breakInfo->callback) j->breakInfo->callback (j); |
---|
| 1192 | // finally destroy the joint if the dJOINT_DELETE_ON_BREAK is set |
---|
| 1193 | if (j->breakInfo->flags & dJOINT_DELETE_ON_BREAK) dJointDestroy (j); |
---|
| 1194 | } |
---|
| 1195 | } |
---|
| 1196 | /*************************************************************************/ |
---|
| 1197 | |
---|
| 1198 | # ifdef TIMING |
---|
| 1199 | dTimerEnd (); |
---|
| 1200 | dTimerReport (stdout, 1); |
---|
| 1201 | # endif |
---|
| 1202 | } |
---|
| 1203 | |
---|
| 1204 | #endif |
---|
| 1205 | |
---|
| 1206 | |
---|
| 1207 | void dWorldStepFast1 (dWorldID w, dReal stepsize, int maxiterations) |
---|
| 1208 | { |
---|
| 1209 | dUASSERT (w, "bad world argument"); |
---|
| 1210 | dUASSERT (stepsize > 0, "stepsize must be > 0"); |
---|
| 1211 | processIslandsFast (w, stepsize, maxiterations); |
---|
| 1212 | } |
---|