1 /* 2 * PURPOSE: 3 * Class representing an articulated rigid body. Stores the body's 4 * current state, allows forces and torques to be set, handles 5 * timestepping and implements Featherstone's algorithm. 6 * 7 * COPYRIGHT: 8 * Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013 9 * Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix) 10 11 This software is provided 'as-is', without any express or implied warranty. 12 In no event will the authors be held liable for any damages arising from the use of this software. 13 Permission is granted to anyone to use this software for any purpose, 14 including commercial applications, and to alter it and redistribute it freely, 15 subject to the following restrictions: 16 17 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. 18 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 19 3. This notice may not be removed or altered from any source distribution. 20 21 */ 22 23 24 #ifndef BT_MULTIBODY_H 25 #define BT_MULTIBODY_H 26 27 #include "LinearMath/btScalar.h" 28 #include "LinearMath/btVector3.h" 29 #include "LinearMath/btQuaternion.h" 30 #include "LinearMath/btMatrix3x3.h" 31 #include "LinearMath/btAlignedObjectArray.h" 32 33 #ifdef BT_USE_DOUBLE_PRECISION 34 #define btMultiBodyData btMultiBodyDoubleData 35 #define btMultiBodyDataName "btMultiBodyDoubleData" 36 #define btMultiBodyLinkData btMultiBodyLinkDoubleData 37 #define btMultiBodyLinkDataName "btMultiBodyLinkDoubleData" 38 #else 39 #define btMultiBodyData btMultiBodyFloatData 40 #define btMultiBodyDataName "btMultiBodyFloatData" 41 #define btMultiBodyLinkData btMultiBodyLinkFloatData 42 #define btMultiBodyLinkDataName "btMultiBodyLinkFloatData" 43 #endif //BT_USE_DOUBLE_PRECISION 44 45 #include "btMultiBodyLink.h" 46 class btMultiBodyLinkCollider; 47 48 class btMultiBody 49 { 50 public: 51 52 53 BT_DECLARE_ALIGNED_ALLOCATOR(); 54 55 // 56 // initialization 57 // 58 59 btMultiBody(int n_links, // NOT including the base 60 btScalar mass, // mass of base 61 const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal 62 bool fixedBase, // whether the base is fixed (true) or can move (false) 63 bool canSleep, 64 bool multiDof = false 65 ); 66 67 68 virtual ~btMultiBody(); 69 70 void setupFixed(int linkIndex, 71 btScalar mass, 72 const btVector3 &inertia, 73 int parent, 74 const btQuaternion &rotParentToThis, 75 const btVector3 &parentComToThisPivotOffset, 76 const btVector3 &thisPivotToThisComOffset, 77 bool disableParentCollision); 78 79 80 void setupPrismatic(int i, 81 btScalar mass, 82 const btVector3 &inertia, 83 int parent, 84 const btQuaternion &rotParentToThis, 85 const btVector3 &jointAxis, 86 const btVector3 &parentComToThisPivotOffset, 87 const btVector3 &thisPivotToThisComOffset, 88 bool disableParentCollision); 89 90 void setupRevolute(int linkIndex, // 0 to num_links-1 91 btScalar mass, 92 const btVector3 &inertia, 93 int parentIndex, 94 const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 95 const btVector3 &jointAxis, // in my frame 96 const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame 97 const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame 98 bool disableParentCollision=false); 99 100 void setupSpherical(int linkIndex, // 0 to num_links-1 101 btScalar mass, 102 const btVector3 &inertia, 103 int parent, 104 const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 105 const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame 106 const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame 107 bool disableParentCollision=false); 108 109 void setupPlanar(int i, // 0 to num_links-1 110 btScalar mass, 111 const btVector3 &inertia, 112 int parent, 113 const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0 114 const btVector3 &rotationAxis, 115 const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame 116 bool disableParentCollision=false); 117 getLink(int index)118 const btMultibodyLink& getLink(int index) const 119 { 120 return m_links[index]; 121 } 122 getLink(int index)123 btMultibodyLink& getLink(int index) 124 { 125 return m_links[index]; 126 } 127 128 setBaseCollider(btMultiBodyLinkCollider * collider)129 void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base 130 { 131 m_baseCollider = collider; 132 } getBaseCollider()133 const btMultiBodyLinkCollider* getBaseCollider() const 134 { 135 return m_baseCollider; 136 } getBaseCollider()137 btMultiBodyLinkCollider* getBaseCollider() 138 { 139 return m_baseCollider; 140 } 141 142 // 143 // get parent 144 // input: link num from 0 to num_links-1 145 // output: link num from 0 to num_links-1, OR -1 to mean the base. 146 // 147 int getParent(int link_num) const; 148 149 150 // 151 // get number of m_links, masses, moments of inertia 152 // 153 getNumLinks()154 int getNumLinks() const { return m_links.size(); } getNumDofs()155 int getNumDofs() const { return m_dofCount; } getNumPosVars()156 int getNumPosVars() const { return m_posVarCnt; } getBaseMass()157 btScalar getBaseMass() const { return m_baseMass; } getBaseInertia()158 const btVector3 & getBaseInertia() const { return m_baseInertia; } 159 btScalar getLinkMass(int i) const; 160 const btVector3 & getLinkInertia(int i) const; 161 162 163 164 // 165 // change mass (incomplete: can only change base mass and inertia at present) 166 // 167 setBaseMass(btScalar mass)168 void setBaseMass(btScalar mass) { m_baseMass = mass; } setBaseInertia(const btVector3 & inertia)169 void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; } 170 171 172 // 173 // get/set pos/vel/rot/omega for the base link 174 // 175 getBasePos()176 const btVector3 & getBasePos() const { return m_basePos; } // in world frame getBaseVel()177 const btVector3 getBaseVel() const 178 { 179 return btVector3(m_realBuf[3],m_realBuf[4],m_realBuf[5]); 180 } // in world frame getWorldToBaseRot()181 const btQuaternion & getWorldToBaseRot() const 182 { 183 return m_baseQuat; 184 } // rotates world vectors into base frame getBaseOmega()185 btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); } // in world frame 186 setBasePos(const btVector3 & pos)187 void setBasePos(const btVector3 &pos) 188 { 189 m_basePos = pos; 190 } 191 setBaseWorldTransform(const btTransform & tr)192 void setBaseWorldTransform(const btTransform& tr) 193 { 194 setBasePos(tr.getOrigin()); 195 setWorldToBaseRot(tr.getRotation().inverse()); 196 197 } 198 getBaseWorldTransform()199 btTransform getBaseWorldTransform() const 200 { 201 btTransform tr; 202 tr.setOrigin(getBasePos()); 203 tr.setRotation(getWorldToBaseRot().inverse()); 204 return tr; 205 } 206 setBaseVel(const btVector3 & vel)207 void setBaseVel(const btVector3 &vel) 208 { 209 210 m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2]; 211 } setWorldToBaseRot(const btQuaternion & rot)212 void setWorldToBaseRot(const btQuaternion &rot) 213 { 214 m_baseQuat = rot; //m_baseQuat asumed to ba alias!? 215 } setBaseOmega(const btVector3 & omega)216 void setBaseOmega(const btVector3 &omega) 217 { 218 m_realBuf[0]=omega[0]; 219 m_realBuf[1]=omega[1]; 220 m_realBuf[2]=omega[2]; 221 } 222 223 224 // 225 // get/set pos/vel for child m_links (i = 0 to num_links-1) 226 // 227 228 btScalar getJointPos(int i) const; 229 btScalar getJointVel(int i) const; 230 231 btScalar * getJointVelMultiDof(int i); 232 btScalar * getJointPosMultiDof(int i); 233 234 const btScalar * getJointVelMultiDof(int i) const ; 235 const btScalar * getJointPosMultiDof(int i) const ; 236 237 void setJointPos(int i, btScalar q); 238 void setJointVel(int i, btScalar qdot); 239 void setJointPosMultiDof(int i, btScalar *q); 240 void setJointVelMultiDof(int i, btScalar *qdot); 241 242 243 244 // 245 // direct access to velocities as a vector of 6 + num_links elements. 246 // (omega first, then v, then joint velocities.) 247 // getVelocityVector()248 const btScalar * getVelocityVector() const 249 { 250 return &m_realBuf[0]; 251 } 252 /* btScalar * getVelocityVector() 253 { 254 return &real_buf[0]; 255 } 256 */ 257 258 // 259 // get the frames of reference (positions and orientations) of the child m_links 260 // (i = 0 to num_links-1) 261 // 262 263 const btVector3 & getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords 264 const btQuaternion & getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i. 265 266 267 // 268 // transform vectors in local frame of link i to world frame (or vice versa) 269 // 270 btVector3 localPosToWorld(int i, const btVector3 &vec) const; 271 btVector3 localDirToWorld(int i, const btVector3 &vec) const; 272 btVector3 worldPosToLocal(int i, const btVector3 &vec) const; 273 btVector3 worldDirToLocal(int i, const btVector3 &vec) const; 274 275 276 // 277 // calculate kinetic energy and angular momentum 278 // useful for debugging. 279 // 280 281 btScalar getKineticEnergy() const; 282 btVector3 getAngularMomentum() const; 283 284 285 // 286 // set external forces and torques. Note all external forces/torques are given in the WORLD frame. 287 // 288 289 void clearForcesAndTorques(); 290 void clearConstraintForces(); 291 292 void clearVelocities(); 293 addBaseForce(const btVector3 & f)294 void addBaseForce(const btVector3 &f) 295 { 296 m_baseForce += f; 297 } addBaseTorque(const btVector3 & t)298 void addBaseTorque(const btVector3 &t) { m_baseTorque += t; } 299 void addLinkForce(int i, const btVector3 &f); 300 void addLinkTorque(int i, const btVector3 &t); 301 addBaseConstraintForce(const btVector3 & f)302 void addBaseConstraintForce(const btVector3 &f) 303 { 304 m_baseConstraintForce += f; 305 } addBaseConstraintTorque(const btVector3 & t)306 void addBaseConstraintTorque(const btVector3 &t) { m_baseConstraintTorque += t; } 307 void addLinkConstraintForce(int i, const btVector3 &f); 308 void addLinkConstraintTorque(int i, const btVector3 &t); 309 310 311 void addJointTorque(int i, btScalar Q); 312 void addJointTorqueMultiDof(int i, int dof, btScalar Q); 313 void addJointTorqueMultiDof(int i, const btScalar *Q); 314 getBaseForce()315 const btVector3 & getBaseForce() const { return m_baseForce; } getBaseTorque()316 const btVector3 & getBaseTorque() const { return m_baseTorque; } 317 const btVector3 & getLinkForce(int i) const; 318 const btVector3 & getLinkTorque(int i) const; 319 btScalar getJointTorque(int i) const; 320 btScalar * getJointTorqueMultiDof(int i); 321 322 323 // 324 // dynamics routines. 325 // 326 327 // timestep the velocities (given the external forces/torques set using addBaseForce etc). 328 // also sets up caches for calcAccelerationDeltas. 329 // 330 // Note: the caller must provide three vectors which are used as 331 // temporary scratch space. The idea here is to reduce dynamic 332 // memory allocation: the same scratch vectors can be re-used 333 // again and again for different Multibodies, instead of each 334 // btMultiBody allocating (and then deallocating) their own 335 // individual scratch buffers. This gives a considerable speed 336 // improvement, at least on Windows (where dynamic memory 337 // allocation appears to be fairly slow). 338 // 339 void stepVelocities(btScalar dt, 340 btAlignedObjectArray<btScalar> &scratch_r, 341 btAlignedObjectArray<btVector3> &scratch_v, 342 btAlignedObjectArray<btMatrix3x3> &scratch_m); 343 344 void stepVelocitiesMultiDof(btScalar dt, 345 btAlignedObjectArray<btScalar> &scratch_r, 346 btAlignedObjectArray<btVector3> &scratch_v, 347 btAlignedObjectArray<btMatrix3x3> &scratch_m, 348 bool isConstraintPass=false 349 ); 350 351 // calcAccelerationDeltas 352 // input: force vector (in same format as jacobian, i.e.: 353 // 3 torque values, 3 force values, num_links joint torque values) 354 // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values 355 // (existing contents of output array are replaced) 356 // stepVelocities must have been called first. 357 void calcAccelerationDeltas(const btScalar *force, btScalar *output, 358 btAlignedObjectArray<btScalar> &scratch_r, 359 btAlignedObjectArray<btVector3> &scratch_v) const; 360 361 void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, 362 btAlignedObjectArray<btScalar> &scratch_r, 363 btAlignedObjectArray<btVector3> &scratch_v) const; 364 365 // apply a delta-vee directly. used in sequential impulses code. applyDeltaVee(const btScalar * delta_vee)366 void applyDeltaVee(const btScalar * delta_vee) 367 { 368 369 for (int i = 0; i < 6 + getNumLinks(); ++i) 370 { 371 m_realBuf[i] += delta_vee[i]; 372 } 373 374 } applyDeltaVee(const btScalar * delta_vee,btScalar multiplier)375 void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier) 376 { 377 btScalar sum = 0; 378 for (int i = 0; i < 6 + getNumLinks(); ++i) 379 { 380 sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier; 381 } 382 btScalar l = btSqrt(sum); 383 /* 384 static btScalar maxl = -1e30f; 385 if (l>maxl) 386 { 387 maxl=l; 388 // printf("maxl=%f\n",maxl); 389 } 390 */ 391 if (l>m_maxAppliedImpulse) 392 { 393 // printf("exceeds 100: l=%f\n",maxl); 394 multiplier *= m_maxAppliedImpulse/l; 395 } 396 397 for (int i = 0; i < 6 + getNumLinks(); ++i) 398 { 399 sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier; 400 m_realBuf[i] += delta_vee[i] * multiplier; 401 btClamp(m_realBuf[i],-m_maxCoordinateVelocity,m_maxCoordinateVelocity); 402 } 403 } 404 applyDeltaVeeMultiDof2(const btScalar * delta_vee,btScalar multiplier)405 void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier) 406 { 407 for (int dof = 0; dof < 6 + getNumDofs(); ++dof) 408 { 409 m_deltaV[dof] += delta_vee[dof] * multiplier; 410 } 411 } processDeltaVeeMultiDof2()412 void processDeltaVeeMultiDof2() 413 { 414 applyDeltaVeeMultiDof(&m_deltaV[0],1); 415 416 for (int dof = 0; dof < 6 + getNumDofs(); ++dof) 417 { 418 m_deltaV[dof] = 0.f; 419 } 420 } 421 applyDeltaVeeMultiDof(const btScalar * delta_vee,btScalar multiplier)422 void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier) 423 { 424 //for (int dof = 0; dof < 6 + getNumDofs(); ++dof) 425 // printf("%.4f ", delta_vee[dof]*multiplier); 426 //printf("\n"); 427 428 //btScalar sum = 0; 429 //for (int dof = 0; dof < 6 + getNumDofs(); ++dof) 430 //{ 431 // sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier; 432 //} 433 //btScalar l = btSqrt(sum); 434 435 //if (l>m_maxAppliedImpulse) 436 //{ 437 // multiplier *= m_maxAppliedImpulse/l; 438 //} 439 440 for (int dof = 0; dof < 6 + getNumDofs(); ++dof) 441 { 442 m_realBuf[dof] += delta_vee[dof] * multiplier; 443 btClamp(m_realBuf[dof],-m_maxCoordinateVelocity,m_maxCoordinateVelocity); 444 } 445 } 446 447 448 449 // timestep the positions (given current velocities). 450 void stepPositions(btScalar dt); 451 void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0); 452 453 454 // 455 // contacts 456 // 457 458 // This routine fills out a contact constraint jacobian for this body. 459 // the 'normal' supplied must be -n for body1 or +n for body2 of the contact. 460 // 'normal' & 'contact_point' are both given in world coordinates. 461 void fillContactJacobian(int link, 462 const btVector3 &contact_point, 463 const btVector3 &normal, 464 btScalar *jac, 465 btAlignedObjectArray<btScalar> &scratch_r, 466 btAlignedObjectArray<btVector3> &scratch_v, 467 btAlignedObjectArray<btMatrix3x3> &scratch_m) const; 468 469 //multidof version of fillContactJacobian fillContactJacobianMultiDof(int link,const btVector3 & contact_point,const btVector3 & normal,btScalar * jac,btAlignedObjectArray<btScalar> & scratch_r,btAlignedObjectArray<btVector3> & scratch_v,btAlignedObjectArray<btMatrix3x3> & scratch_m)470 void fillContactJacobianMultiDof(int link, 471 const btVector3 &contact_point, 472 const btVector3 &normal, 473 btScalar *jac, 474 btAlignedObjectArray<btScalar> &scratch_r, 475 btAlignedObjectArray<btVector3> &scratch_v, 476 btAlignedObjectArray<btMatrix3x3> &scratch_m) const { filConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); } 477 478 //a more general version of fillContactJacobianMultiDof which does not assume.. 479 //.. that the constraint in question is contact or, to be more precise, constrains linear velocity only 480 void filConstraintJacobianMultiDof(int link, 481 const btVector3 &contact_point, 482 const btVector3 &normal_ang, 483 const btVector3 &normal_lin, 484 btScalar *jac, 485 btAlignedObjectArray<btScalar> &scratch_r, 486 btAlignedObjectArray<btVector3> &scratch_v, 487 btAlignedObjectArray<btMatrix3x3> &scratch_m) const; 488 489 490 // 491 // sleeping 492 // setCanSleep(bool canSleep)493 void setCanSleep(bool canSleep) 494 { 495 m_canSleep = canSleep; 496 } 497 getCanSleep()498 bool getCanSleep()const 499 { 500 return m_canSleep; 501 } 502 isAwake()503 bool isAwake() const { return m_awake; } 504 void wakeUp(); 505 void goToSleep(); 506 void checkMotionAndSleepIfRequired(btScalar timestep); 507 hasFixedBase()508 bool hasFixedBase() const 509 { 510 return m_fixedBase; 511 } 512 getCompanionId()513 int getCompanionId() const 514 { 515 return m_companionId; 516 } setCompanionId(int id)517 void setCompanionId(int id) 518 { 519 //printf("for %p setCompanionId(%d)\n",this, id); 520 m_companionId = id; 521 } 522 setNumLinks(int numLinks)523 void setNumLinks(int numLinks)//careful: when changing the number of m_links, make sure to re-initialize or update existing m_links 524 { 525 m_links.resize(numLinks); 526 } 527 getLinearDamping()528 btScalar getLinearDamping() const 529 { 530 return m_linearDamping; 531 } setLinearDamping(btScalar damp)532 void setLinearDamping( btScalar damp) 533 { 534 m_linearDamping = damp; 535 } getAngularDamping()536 btScalar getAngularDamping() const 537 { 538 return m_angularDamping; 539 } setAngularDamping(btScalar damp)540 void setAngularDamping( btScalar damp) 541 { 542 m_angularDamping = damp; 543 } 544 getUseGyroTerm()545 bool getUseGyroTerm() const 546 { 547 return m_useGyroTerm; 548 } setUseGyroTerm(bool useGyro)549 void setUseGyroTerm(bool useGyro) 550 { 551 m_useGyroTerm = useGyro; 552 } getMaxCoordinateVelocity()553 btScalar getMaxCoordinateVelocity() const 554 { 555 return m_maxCoordinateVelocity ; 556 } setMaxCoordinateVelocity(btScalar maxVel)557 void setMaxCoordinateVelocity(btScalar maxVel) 558 { 559 m_maxCoordinateVelocity = maxVel; 560 } 561 getMaxAppliedImpulse()562 btScalar getMaxAppliedImpulse() const 563 { 564 return m_maxAppliedImpulse; 565 } setMaxAppliedImpulse(btScalar maxImp)566 void setMaxAppliedImpulse(btScalar maxImp) 567 { 568 m_maxAppliedImpulse = maxImp; 569 } setHasSelfCollision(bool hasSelfCollision)570 void setHasSelfCollision(bool hasSelfCollision) 571 { 572 m_hasSelfCollision = hasSelfCollision; 573 } hasSelfCollision()574 bool hasSelfCollision() const 575 { 576 return m_hasSelfCollision; 577 } 578 isMultiDof()579 bool isMultiDof() { return m_isMultiDof; } 580 void finalizeMultiDof(); 581 useRK4Integration(bool use)582 void useRK4Integration(bool use) { m_useRK4 = use; } isUsingRK4Integration()583 bool isUsingRK4Integration() const { return m_useRK4; } useGlobalVelocities(bool use)584 void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; } isUsingGlobalVelocities()585 bool isUsingGlobalVelocities() const { return m_useGlobalVelocities; } 586 isPosUpdated()587 bool isPosUpdated() const 588 { 589 return __posUpdated; 590 } setPosUpdated(bool updated)591 void setPosUpdated(bool updated) 592 { 593 __posUpdated = updated; 594 } 595 596 //internalNeedsJointFeedback is for internal use only internalNeedsJointFeedback()597 bool internalNeedsJointFeedback() const 598 { 599 return m_internalNeedsJointFeedback; 600 } 601 void forwardKinematics(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m); 602 603 void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m); 604 605 virtual int calculateSerializeBufferSize() const; 606 607 ///fills the dataBuffer and returns the struct name (and 0 on failure) 608 virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const; 609 getBaseName()610 const char* getBaseName() const 611 { 612 return m_baseName; 613 } 614 ///memory of setBaseName needs to be manager by user setBaseName(const char * name)615 void setBaseName(const char* name) 616 { 617 m_baseName = name; 618 } 619 620 private: 621 btMultiBody(const btMultiBody &); // not implemented 622 void operator=(const btMultiBody &); // not implemented 623 624 void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const; 625 626 void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const; 627 void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const; 628 updateLinksDofOffsets()629 void updateLinksDofOffsets() 630 { 631 int dofOffset = 0, cfgOffset = 0; 632 for(int bidx = 0; bidx < m_links.size(); ++bidx) 633 { 634 m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset; 635 dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount; 636 } 637 } 638 639 void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const; 640 641 642 private: 643 644 btMultiBodyLinkCollider* m_baseCollider;//can be NULL 645 const char* m_baseName;//memory needs to be manager by user! 646 647 btVector3 m_basePos; // position of COM of base (world frame) 648 btQuaternion m_baseQuat; // rotates world points into base frame 649 650 btScalar m_baseMass; // mass of the base 651 btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal) 652 653 btVector3 m_baseForce; // external force applied to base. World frame. 654 btVector3 m_baseTorque; // external torque applied to base. World frame. 655 656 btVector3 m_baseConstraintForce; // external force applied to base. World frame. 657 btVector3 m_baseConstraintTorque; // external torque applied to base. World frame. 658 659 btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1. 660 btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders; 661 662 663 // 664 // realBuf: 665 // offset size array 666 // 0 6 + num_links v (base_omega; base_vel; joint_vels) MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized] 667 // 6+num_links num_links D 668 // 669 // vectorBuf: 670 // offset size array 671 // 0 num_links h_top 672 // num_links num_links h_bottom 673 // 674 // matrixBuf: 675 // offset size array 676 // 0 num_links+1 rot_from_parent 677 // 678 btAlignedObjectArray<btScalar> m_deltaV; 679 btAlignedObjectArray<btScalar> m_realBuf; 680 btAlignedObjectArray<btVector3> m_vectorBuf; 681 btAlignedObjectArray<btMatrix3x3> m_matrixBuf; 682 683 684 btMatrix3x3 m_cachedInertiaTopLeft; 685 btMatrix3x3 m_cachedInertiaTopRight; 686 btMatrix3x3 m_cachedInertiaLowerLeft; 687 btMatrix3x3 m_cachedInertiaLowerRight; 688 689 bool m_fixedBase; 690 691 // Sleep parameters. 692 bool m_awake; 693 bool m_canSleep; 694 btScalar m_sleepTimer; 695 696 int m_companionId; 697 btScalar m_linearDamping; 698 btScalar m_angularDamping; 699 bool m_useGyroTerm; 700 btScalar m_maxAppliedImpulse; 701 btScalar m_maxCoordinateVelocity; 702 bool m_hasSelfCollision; 703 bool m_isMultiDof; 704 bool __posUpdated; 705 int m_dofCount, m_posVarCnt; 706 bool m_useRK4, m_useGlobalVelocities; 707 708 ///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only 709 bool m_internalNeedsJointFeedback; 710 }; 711 712 struct btMultiBodyLinkDoubleData 713 { 714 btQuaternionDoubleData m_zeroRotParentToThis; 715 btVector3DoubleData m_parentComToThisComOffset; 716 btVector3DoubleData m_thisPivotToThisComOffset; 717 btVector3DoubleData m_jointAxisTop[6]; 718 btVector3DoubleData m_jointAxisBottom[6]; 719 720 721 char *m_linkName; 722 char *m_jointName; 723 btCollisionObjectDoubleData *m_linkCollider; 724 725 btVector3DoubleData m_linkInertia; // inertia of the base (in local frame; diagonal) 726 double m_linkMass; 727 int m_parentIndex; 728 int m_jointType; 729 730 731 732 733 int m_dofCount; 734 int m_posVarCount; 735 double m_jointPos[7]; 736 double m_jointVel[6]; 737 double m_jointTorque[6]; 738 739 740 741 }; 742 743 struct btMultiBodyLinkFloatData 744 { 745 btQuaternionFloatData m_zeroRotParentToThis; 746 btVector3FloatData m_parentComToThisComOffset; 747 btVector3FloatData m_thisPivotToThisComOffset; 748 btVector3FloatData m_jointAxisTop[6]; 749 btVector3FloatData m_jointAxisBottom[6]; 750 751 752 char *m_linkName; 753 char *m_jointName; 754 btCollisionObjectFloatData *m_linkCollider; 755 756 btVector3FloatData m_linkInertia; // inertia of the base (in local frame; diagonal) 757 int m_dofCount; 758 float m_linkMass; 759 int m_parentIndex; 760 int m_jointType; 761 762 763 764 float m_jointPos[7]; 765 float m_jointVel[6]; 766 float m_jointTorque[6]; 767 int m_posVarCount; 768 769 770 }; 771 772 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 773 struct btMultiBodyDoubleData 774 { 775 char *m_baseName; 776 btMultiBodyLinkDoubleData *m_links; 777 btCollisionObjectDoubleData *m_baseCollider; 778 779 btTransformDoubleData m_baseWorldTransform; 780 btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal) 781 782 int m_numLinks; 783 double m_baseMass; 784 785 char m_padding[4]; 786 787 }; 788 789 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 790 struct btMultiBodyFloatData 791 { 792 char *m_baseName; 793 btMultiBodyLinkFloatData *m_links; 794 btCollisionObjectFloatData *m_baseCollider; 795 btTransformFloatData m_baseWorldTransform; 796 btVector3FloatData m_baseInertia; // inertia of the base (in local frame; diagonal) 797 798 float m_baseMass; 799 int m_numLinks; 800 }; 801 802 803 804 #endif 805