~ubuntu-branches/ubuntu/vivid/emscripten/vivid

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.

2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.

3. This notice may not be removed or altered from any source distribution.

#include "btDiscreteDynamicsWorld.h"

//collision detection

#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"

#include "BulletCollision/BroadphaseCollision/btSimpleBroadphase.h"

#include "BulletCollision/BroadphaseCollision/btCollisionAlgorithm.h"

#include "BulletCollision/CollisionShapes/btCollisionShape.h"

#include "BulletCollision/CollisionDispatch/btSimulationIslandManager.h"

#include "LinearMath/btTransformUtil.h"

#include "LinearMath/btQuickprof.h"

//rigidbody & constraints

#include "BulletDynamics/Dynamics/btRigidBody.h"

#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"

#include "BulletDynamics/ConstraintSolver/btContactSolverInfo.h"

#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"

#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"

#include "BulletDynamics/ConstraintSolver/btHingeConstraint.h"

#include "BulletDynamics/ConstraintSolver/btConeTwistConstraint.h"

#include "BulletDynamics/ConstraintSolver/btGeneric6DofConstraint.h"

#include "BulletDynamics/ConstraintSolver/btSliderConstraint.h"

#include "BulletDynamics/ConstraintSolver/btContactConstraint.h"

#include "LinearMath/btIDebugDraw.h"

#include "BulletCollision/CollisionShapes/btSphereShape.h"

#include "BulletDynamics/Dynamics/btActionInterface.h"

#include "LinearMath/btQuickprof.h"

#include "LinearMath/btMotionState.h"

#include "LinearMath/btSerializer.h"

#if 0

btAlignedObjectArray<btVector3> debugContacts;

btAlignedObjectArray<btVector3> debugNormals;

int startHit=2;

int firstHit=startHit;

#endif

btDiscreteDynamicsWorld::btDiscreteDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btConstraintSolver* constraintSolver, btCollisionConfiguration* collisionConfiguration)

:btDynamicsWorld(dispatcher,pairCache,collisionConfiguration),

m_constraintSolver(constraintSolver),

m_gravity(0,-10,0),

m_localTime(0),

m_synchronizeAllMotionStates(false),

m_profileTimings(0)

{

if (!m_constraintSolver)

{

void* mem = btAlignedAlloc(sizeof(btSequentialImpulseConstraintSolver),16);

m_constraintSolver = new (mem) btSequentialImpulseConstraintSolver;

m_ownsConstraintSolver = true;

} else

{

m_ownsConstraintSolver = false;

}

{

void* mem = btAlignedAlloc(sizeof(btSimulationIslandManager),16);

m_islandManager = new (mem) btSimulationIslandManager();

}

m_ownsIslandManager = true;

}

btDiscreteDynamicsWorld::~btDiscreteDynamicsWorld()

{

//only delete it when we created it

if (m_ownsIslandManager)

{

m_islandManager->~btSimulationIslandManager();

btAlignedFree( m_islandManager);

}

if (m_ownsConstraintSolver)

{

m_constraintSolver->~btConstraintSolver();

btAlignedFree(m_constraintSolver);

100

}

101

}

102

103

void btDiscreteDynamicsWorld::saveKinematicState(btScalar timeStep)

104

{

105

///would like to iterate over m_nonStaticRigidBodies, but unfortunately old API allows

106

///to switch status _after_ adding kinematic objects to the world

107

///fix it for Bullet 3.x release

108

for (int i=0;i<m_collisionObjects.size();i++)

109

{

110

btCollisionObject* colObj = m_collisionObjects[i];

111

btRigidBody* body = btRigidBody::upcast(colObj);

112

if (body && body->getActivationState() != ISLAND_SLEEPING)

113

{

114

if (body->isKinematicObject())

115

{

116

//to calculate velocities next frame

117

body->saveKinematicState(timeStep);

118

}

119

}

120

}

121

122

}

123

124

void btDiscreteDynamicsWorld::debugDrawWorld()

125

{

126

BT_PROFILE("debugDrawWorld");

127

128

btCollisionWorld::debugDrawWorld();

129

130

bool drawConstraints = false;

131

if (getDebugDrawer())

132

{

133

int mode = getDebugDrawer()->getDebugMode();

134

if(mode & (btIDebugDraw::DBG_DrawConstraints | btIDebugDraw::DBG_DrawConstraintLimits))

135

{

136

drawConstraints = true;

137

}

138

}

139

if(drawConstraints)

140

{

141

for(int i = getNumConstraints()-1; i>=0 ;i--)

142

{

143

btTypedConstraint* constraint = getConstraint(i);

144

debugDrawConstraint(constraint);

145

}

146

}

147

148

149

150

if (getDebugDrawer() && getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe | btIDebugDraw::DBG_DrawAabb))

151

{

152

int i;

153

154

if (getDebugDrawer() && getDebugDrawer()->getDebugMode())

155

{

156

for (i=0;i<m_actions.size();i++)

157

{

158

m_actions[i]->debugDraw(m_debugDrawer);

159

}

160

}

161

}

162

}

163

164

void btDiscreteDynamicsWorld::clearForces()

165

{

166

///@todo: iterate over awake simulation islands!

167

for ( int i=0;i<m_nonStaticRigidBodies.size();i++)

168

{

169

btRigidBody* body = m_nonStaticRigidBodies[i];

170

//need to check if next line is ok

171

//it might break backward compatibility (people applying forces on sleeping objects get never cleared and accumulate on wake-up

172

body->clearForces();

173

}

174

}

175

176

///apply gravity, call this once per timestep

177

void btDiscreteDynamicsWorld::applyGravity()

178

{

179

///@todo: iterate over awake simulation islands!

180

for ( int i=0;i<m_nonStaticRigidBodies.size();i++)

181

{

182

btRigidBody* body = m_nonStaticRigidBodies[i];

183

if (body->isActive())

184

{

185

body->applyGravity();

186

}

187

}

188

}

189

190

191

void btDiscreteDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)

192

{

193

btAssert(body);

194

195

if (body->getMotionState() && !body->isStaticOrKinematicObject())

196

{

197

//we need to call the update at least once, even for sleeping objects

198

//otherwise the 'graphics' transform never updates properly

199

///@todo: add 'dirty' flag

200

//if (body->getActivationState() != ISLAND_SLEEPING)

201

{

202

btTransform interpolatedTransform;

203

btTransformUtil::integrateTransform(body->getInterpolationWorldTransform(),

204

body->getInterpolationLinearVelocity(),body->getInterpolationAngularVelocity(),m_localTime*body->getHitFraction(),interpolatedTransform);

205

body->getMotionState()->setWorldTransform(interpolatedTransform);

206

}

207

}

208

}

209

210

211

void btDiscreteDynamicsWorld::synchronizeMotionStates()

212

{

213

BT_PROFILE("synchronizeMotionStates");

214

if (m_synchronizeAllMotionStates)

215

{

216

//iterate over all collision objects

217

for ( int i=0;i<m_collisionObjects.size();i++)

218

{

219

btCollisionObject* colObj = m_collisionObjects[i];

220

btRigidBody* body = btRigidBody::upcast(colObj);

221

if (body)

222

synchronizeSingleMotionState(body);

223

}

224

} else

225

{

226

//iterate over all active rigid bodies

227

for ( int i=0;i<m_nonStaticRigidBodies.size();i++)

228

{

229

btRigidBody* body = m_nonStaticRigidBodies[i];

230

if (body->isActive())

231

synchronizeSingleMotionState(body);

232

}

233

}

234

}

235

236

237

int btDiscreteDynamicsWorld::stepSimulation( btScalar timeStep,int maxSubSteps, btScalar fixedTimeStep)

238

{

239

startProfiling(timeStep);

240

241

BT_PROFILE("stepSimulation");

242

243

int numSimulationSubSteps = 0;

244

245

if (maxSubSteps)

246

{

247

//fixed timestep with interpolation

248

m_localTime += timeStep;

249

if (m_localTime >= fixedTimeStep)

250

{

251

numSimulationSubSteps = int( m_localTime / fixedTimeStep);

252

m_localTime -= numSimulationSubSteps * fixedTimeStep;

253

}

254

} else

255

{

256

//variable timestep

257

fixedTimeStep = timeStep;

258

m_localTime = timeStep;

259

if (btFuzzyZero(timeStep))

260

{

261

numSimulationSubSteps = 0;

262

maxSubSteps = 0;

263

} else

264

{

265

numSimulationSubSteps = 1;

266

maxSubSteps = 1;

267

}

268

}

269

270

//process some debugging flags

271

if (getDebugDrawer())

272

{

273

btIDebugDraw* debugDrawer = getDebugDrawer ();

274

gDisableDeactivation = (debugDrawer->getDebugMode() & btIDebugDraw::DBG_NoDeactivation) != 0;

275

}

276

if (numSimulationSubSteps)

277

{

278

279

//clamp the number of substeps, to prevent simulation grinding spiralling down to a halt

280

int clampedSimulationSteps = (numSimulationSubSteps > maxSubSteps)? maxSubSteps : numSimulationSubSteps;

281

282

saveKinematicState(fixedTimeStep*clampedSimulationSteps);

283

284

applyGravity();

285

286

287

288

for (int i=0;i<clampedSimulationSteps;i++)

289

{

290

internalSingleStepSimulation(fixedTimeStep);

291

synchronizeMotionStates();

292

}

293

294

} else

295

{

296

synchronizeMotionStates();

297

}

298

299

clearForces();

300

301

#ifndef BT_NO_PROFILE

302

CProfileManager::Increment_Frame_Counter();

303

#endif //BT_NO_PROFILE

304

305

return numSimulationSubSteps;

306

}

307

308

void btDiscreteDynamicsWorld::internalSingleStepSimulation(btScalar timeStep)

309

{

310

311

BT_PROFILE("internalSingleStepSimulation");

312

313

if(0 != m_internalPreTickCallback) {

314

(*m_internalPreTickCallback)(this, timeStep);

315

}

316

317

///apply gravity, predict motion

318

predictUnconstraintMotion(timeStep);

319

320

btDispatcherInfo& dispatchInfo = getDispatchInfo();

321

322

dispatchInfo.m_timeStep = timeStep;

323

dispatchInfo.m_stepCount = 0;

324

dispatchInfo.m_debugDraw = getDebugDrawer();

325

326

327

///perform collision detection

328

performDiscreteCollisionDetection();

329

330

if (getDispatchInfo().m_useContinuous)

331

addSpeculativeContacts(timeStep);

332

333

334

calculateSimulationIslands();

335

336

337

getSolverInfo().m_timeStep = timeStep;

338

339

340

341

///solve contact and other joint constraints

342

solveConstraints(getSolverInfo());

343

344

///CallbackTriggers();

345

346

///integrate transforms

347

integrateTransforms(timeStep);

348

349

///update vehicle simulation

350

updateActions(timeStep);

351

352

updateActivationState( timeStep );

353

354

if(0 != m_internalTickCallback) {

355

(*m_internalTickCallback)(this, timeStep);

356

}

357

}

358

359

void btDiscreteDynamicsWorld::setGravity(const btVector3& gravity)

360

{

361

m_gravity = gravity;

362

for ( int i=0;i<m_nonStaticRigidBodies.size();i++)

363

{

364

btRigidBody* body = m_nonStaticRigidBodies[i];

365

if (body->isActive() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))

366

{

367

body->setGravity(gravity);

368

}

369

}

370

}

371

372

btVector3 btDiscreteDynamicsWorld::getGravity () const

373

{

374

return m_gravity;

375

}

376

377

void btDiscreteDynamicsWorld::addCollisionObject(btCollisionObject* collisionObject,short int collisionFilterGroup,short int collisionFilterMask)

378

{

379

btCollisionWorld::addCollisionObject(collisionObject,collisionFilterGroup,collisionFilterMask);

380

}

381

382

void btDiscreteDynamicsWorld::removeCollisionObject(btCollisionObject* collisionObject)

383

{

384

btRigidBody* body = btRigidBody::upcast(collisionObject);

385

if (body)

386

removeRigidBody(body);

387

else

388

btCollisionWorld::removeCollisionObject(collisionObject);

389

}

390

391

void btDiscreteDynamicsWorld::removeRigidBody(btRigidBody* body)

392

{

393

m_nonStaticRigidBodies.remove(body);

394

btCollisionWorld::removeCollisionObject(body);

395

}

396

397

398

void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body)

399

{

400

if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))

401

{

402

body->setGravity(m_gravity);

403

}

404

405

if (body->getCollisionShape())

406

{

407

if (!body->isStaticObject())

408

{

409

m_nonStaticRigidBodies.push_back(body);

410

} else

411

{

412

body->setActivationState(ISLAND_SLEEPING);

413

}

414

415

bool isDynamic = !(body->isStaticObject() || body->isKinematicObject());

416

short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);

417

short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);

418

419

addCollisionObject(body,collisionFilterGroup,collisionFilterMask);

420

}

421

}

422

423

void btDiscreteDynamicsWorld::addRigidBody(btRigidBody* body, short group, short mask)

424

{

425

if (!body->isStaticOrKinematicObject() && !(body->getFlags() &BT_DISABLE_WORLD_GRAVITY))

426

{

427

body->setGravity(m_gravity);

428

}

429

430

if (body->getCollisionShape())

431

{

432

if (!body->isStaticObject())

433

{

434

m_nonStaticRigidBodies.push_back(body);

435

}

436

else

437

{

438

body->setActivationState(ISLAND_SLEEPING);

439

}

440

addCollisionObject(body,group,mask);

441

}

442

}

443

444

445

void btDiscreteDynamicsWorld::updateActions(btScalar timeStep)

446

{

447

BT_PROFILE("updateActions");

448

449

for ( int i=0;i<m_actions.size();i++)

450

{

451

m_actions[i]->updateAction( this, timeStep);

452

}

453

}

454

455

456

void btDiscreteDynamicsWorld::updateActivationState(btScalar timeStep)

457

{

458

BT_PROFILE("updateActivationState");

459

460

for ( int i=0;i<m_nonStaticRigidBodies.size();i++)

461

{

462

btRigidBody* body = m_nonStaticRigidBodies[i];

463

if (body)

464

{

465

body->updateDeactivation(timeStep);

466

467

if (body->wantsSleeping())

468

{

469

if (body->isStaticOrKinematicObject())

470

{

471

body->setActivationState(ISLAND_SLEEPING);

472

} else

473

{

474

if (body->getActivationState() == ACTIVE_TAG)

475

body->setActivationState( WANTS_DEACTIVATION );

476

if (body->getActivationState() == ISLAND_SLEEPING)

477

{

478

body->setAngularVelocity(btVector3(0,0,0));

479

body->setLinearVelocity(btVector3(0,0,0));

480

}

481

482

}

483

} else

484

{

485

if (body->getActivationState() != DISABLE_DEACTIVATION)

486

body->setActivationState( ACTIVE_TAG );

487

}

488

}

489

}

490

}

491

492

void btDiscreteDynamicsWorld::addConstraint(btTypedConstraint* constraint,bool disableCollisionsBetweenLinkedBodies)

493

{

494

m_constraints.push_back(constraint);

495

if (disableCollisionsBetweenLinkedBodies)

496

{

497

constraint->getRigidBodyA().addConstraintRef(constraint);

498

constraint->getRigidBodyB().addConstraintRef(constraint);

499

}

500

}

501

502

void btDiscreteDynamicsWorld::removeConstraint(btTypedConstraint* constraint)

503

{

504

m_constraints.remove(constraint);

505

constraint->getRigidBodyA().removeConstraintRef(constraint);

506

constraint->getRigidBodyB().removeConstraintRef(constraint);

507

}

508

509

void btDiscreteDynamicsWorld::addAction(btActionInterface* action)

510

{

511

m_actions.push_back(action);

512

}

513

514

void btDiscreteDynamicsWorld::removeAction(btActionInterface* action)

515

{

516

m_actions.remove(action);

517

}

518

519

520

void btDiscreteDynamicsWorld::addVehicle(btActionInterface* vehicle)

521

{

522

addAction(vehicle);

523

}

524

525

void btDiscreteDynamicsWorld::removeVehicle(btActionInterface* vehicle)

526

{

527

removeAction(vehicle);

528

}

529

530

void btDiscreteDynamicsWorld::addCharacter(btActionInterface* character)

531

{

532

addAction(character);

533

}

534

535

void btDiscreteDynamicsWorld::removeCharacter(btActionInterface* character)

536

{

537

removeAction(character);

538

}

539

540

541

SIMD_FORCE_INLINE int btGetConstraintIslandId(const btTypedConstraint* lhs)

542

{

543

int islandId;

544

545

const btCollisionObject& rcolObj0 = lhs->getRigidBodyA();

546

const btCollisionObject& rcolObj1 = lhs->getRigidBodyB();

547

islandId= rcolObj0.getIslandTag()>=0?rcolObj0.getIslandTag():rcolObj1.getIslandTag();

548

return islandId;

549

550

}

551

552

553

class btSortConstraintOnIslandPredicate

554

{

555

public:

556

557

bool operator() ( const btTypedConstraint* lhs, const btTypedConstraint* rhs )

558

{

559

int rIslandId0,lIslandId0;

560

rIslandId0 = btGetConstraintIslandId(rhs);

561

lIslandId0 = btGetConstraintIslandId(lhs);

562

return lIslandId0 < rIslandId0;

563

}

564

};

565

566

567

568

void btDiscreteDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)

569

{

570

BT_PROFILE("solveConstraints");

571

572

struct InplaceSolverIslandCallback : public btSimulationIslandManager::IslandCallback

573

{

574

575

btContactSolverInfo& m_solverInfo;

576

btConstraintSolver* m_solver;

577

btTypedConstraint** m_sortedConstraints;

578

int m_numConstraints;

579

btIDebugDraw* m_debugDrawer;

580

btStackAlloc* m_stackAlloc;

581

btDispatcher* m_dispatcher;

582

583

btAlignedObjectArray<btCollisionObject*> m_bodies;

584

btAlignedObjectArray<btPersistentManifold*> m_manifolds;

585

btAlignedObjectArray<btTypedConstraint*> m_constraints;

586

587

588

InplaceSolverIslandCallback(

589

btContactSolverInfo& solverInfo,

590

btConstraintSolver* solver,

591

btTypedConstraint** sortedConstraints,

592

int numConstraints,

593

btIDebugDraw* debugDrawer,

594

btStackAlloc* stackAlloc,

595

btDispatcher* dispatcher)

596

:m_solverInfo(solverInfo),

597

m_solver(solver),

598

m_sortedConstraints(sortedConstraints),

599

m_numConstraints(numConstraints),

600

m_debugDrawer(debugDrawer),

601

m_stackAlloc(stackAlloc),

602

m_dispatcher(dispatcher)

603

{

604

605

}

606

607

608

InplaceSolverIslandCallback& operator=(InplaceSolverIslandCallback& other)

609

{

610

btAssert(0);

611

(void)other;

612

return *this;

613

}

614

virtual void ProcessIsland(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifolds,int numManifolds, int islandId)

615

{

616

if (islandId<0)

617

{

618

if (numManifolds + m_numConstraints)

619

{

620

///we don't split islands, so all constraints/contact manifolds/bodies are passed into the solver regardless the island id

621

m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,&m_sortedConstraints[0],m_numConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);

622

}

623

} else

624

{

625

//also add all non-contact constraints/joints for this island

626

btTypedConstraint** startConstraint = 0;

627

int numCurConstraints = 0;

628

int i;

629

630

//find the first constraint for this island

631

for (i=0;i<m_numConstraints;i++)

632

{

633

if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)

634

{

635

startConstraint = &m_sortedConstraints[i];

636

break;

637

}

638

}

639

//count the number of constraints in this island

640

for (;i<m_numConstraints;i++)

641

{

642

if (btGetConstraintIslandId(m_sortedConstraints[i]) == islandId)

643

{

644

numCurConstraints++;

645

}

646

}

647

648

if (m_solverInfo.m_minimumSolverBatchSize<=1)

649

{

650

///only call solveGroup if there is some work: avoid virtual function call, its overhead can be excessive

651

if (numManifolds + numCurConstraints)

652

{

653

m_solver->solveGroup( bodies,numBodies,manifolds, numManifolds,startConstraint,numCurConstraints,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);

654

}

655

} else

656

{

657

658

for (i=0;i<numBodies;i++)

659

m_bodies.push_back(bodies[i]);

660

for (i=0;i<numManifolds;i++)

661

m_manifolds.push_back(manifolds[i]);

662

for (i=0;i<numCurConstraints;i++)

663

m_constraints.push_back(startConstraint[i]);

664

if ((m_constraints.size()+m_manifolds.size())>m_solverInfo.m_minimumSolverBatchSize)

665

{

666

processConstraints();

667

} else

668

{

669

//printf("deferred\n");

670

}

671

}

672

}

673

}

674

void processConstraints()

675

{

676

if (m_manifolds.size() + m_constraints.size()>0)

677

{

678

m_solver->solveGroup( &m_bodies[0],m_bodies.size(), &m_manifolds[0], m_manifolds.size(), &m_constraints[0], m_constraints.size() ,m_solverInfo,m_debugDrawer,m_stackAlloc,m_dispatcher);

679

}

680

m_bodies.resize(0);

681

m_manifolds.resize(0);

682

m_constraints.resize(0);

683

684

}

685

686

};

687

688

689

690

//sorted version of all btTypedConstraint, based on islandId

691

btAlignedObjectArray<btTypedConstraint*> sortedConstraints;

692

sortedConstraints.resize( m_constraints.size());

693

int i;

694

for (i=0;i<getNumConstraints();i++)

695

{

696

sortedConstraints[i] = m_constraints[i];

697

}

698

699

// btAssert(0);

700

701

702

703

sortedConstraints.quickSort(btSortConstraintOnIslandPredicate());

704

705

btTypedConstraint** constraintsPtr = getNumConstraints() ? &sortedConstraints[0] : 0;

706

707

InplaceSolverIslandCallback solverCallback( solverInfo, m_constraintSolver, constraintsPtr,sortedConstraints.size(), m_debugDrawer,m_stackAlloc,m_dispatcher1);

708

709

m_constraintSolver->prepareSolve(getCollisionWorld()->getNumCollisionObjects(), getCollisionWorld()->getDispatcher()->getNumManifolds());

710

711

/// solve all the constraints for this island

712

m_islandManager->buildAndProcessIslands(getCollisionWorld()->getDispatcher(),getCollisionWorld(),&solverCallback);

713

714

solverCallback.processConstraints();

715

716

m_constraintSolver->allSolved(solverInfo, m_debugDrawer, m_stackAlloc);

717

}

718

719

720

721

722

void btDiscreteDynamicsWorld::calculateSimulationIslands()

723

{

724

BT_PROFILE("calculateSimulationIslands");

725

726

getSimulationIslandManager()->updateActivationState(getCollisionWorld(),getCollisionWorld()->getDispatcher());

727

728

{

729

int i;

730

int numConstraints = int(m_constraints.size());

731

for (i=0;i< numConstraints ; i++ )

732

{

733

btTypedConstraint* constraint = m_constraints[i];

734

735

const btRigidBody* colObj0 = &constraint->getRigidBodyA();

736

const btRigidBody* colObj1 = &constraint->getRigidBodyB();

737

738

if (((colObj0) && (!(colObj0)->isStaticOrKinematicObject())) &&

739

((colObj1) && (!(colObj1)->isStaticOrKinematicObject())))

740

{

741

if (colObj0->isActive() || colObj1->isActive())

742

{

743

744

getSimulationIslandManager()->getUnionFind().unite((colObj0)->getIslandTag(),

745

(colObj1)->getIslandTag());

746

}

747

}

748

}

749

}

750

751

//Store the island id in each body

752

getSimulationIslandManager()->storeIslandActivationState(getCollisionWorld());

753

754

755

}

756

757

758

759

760

class btClosestNotMeConvexResultCallback : public btCollisionWorld::ClosestConvexResultCallback

761

{

762

public:

763

764

btCollisionObject* m_me;

765

btScalar m_allowedPenetration;

766

btOverlappingPairCache* m_pairCache;

767

btDispatcher* m_dispatcher;

768

769

public:

770

btClosestNotMeConvexResultCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :

771

btCollisionWorld::ClosestConvexResultCallback(fromA,toA),

772

m_me(me),

773

m_allowedPenetration(0.0f),

774

m_pairCache(pairCache),

775

m_dispatcher(dispatcher)

776

{

777

}

778

779

virtual btScalar addSingleResult(btCollisionWorld::LocalConvexResult& convexResult,bool normalInWorldSpace)

780

{

781

if (convexResult.m_hitCollisionObject == m_me)

782

return 1.0f;

783

784

//ignore result if there is no contact response

785

if(!convexResult.m_hitCollisionObject->hasContactResponse())

786

return 1.0f;

787

788

btVector3 linVelA,linVelB;

789

linVelA = m_convexToWorld-m_convexFromWorld;

790

linVelB = btVector3(0,0,0);//toB.getOrigin()-fromB.getOrigin();

791

792

btVector3 relativeVelocity = (linVelA-linVelB);

793

//don't report time of impact for motion away from the contact normal (or causes minor penetration)

794

if (convexResult.m_hitNormalLocal.dot(relativeVelocity)>=-m_allowedPenetration)

795

return 1.f;

796

797

return ClosestConvexResultCallback::addSingleResult (convexResult, normalInWorldSpace);

798

}

799

800

virtual bool needsCollision(btBroadphaseProxy* proxy0) const

801

{

802

//don't collide with itself

803

if (proxy0->m_clientObject == m_me)

804

return false;

805

806

///don't do CCD when the collision filters are not matching

807

if (!ClosestConvexResultCallback::needsCollision(proxy0))

808

return false;

809

810

btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;

811

812

//call needsResponse, see http://code.google.com/p/bullet/issues/detail?id=179

813

if (m_dispatcher->needsResponse(m_me,otherObj))

814

{

815

#if 0

816

///don't do CCD when there are already contact points (touching contact/penetration)

817

btAlignedObjectArray<btPersistentManifold*> manifoldArray;

818

btBroadphasePair* collisionPair = m_pairCache->findPair(m_me->getBroadphaseHandle(),proxy0);

819

if (collisionPair)

820

{

821

if (collisionPair->m_algorithm)

822

{

823

manifoldArray.resize(0);

824

collisionPair->m_algorithm->getAllContactManifolds(manifoldArray);

825

for (int j=0;j<manifoldArray.size();j++)

826

{

827

btPersistentManifold* manifold = manifoldArray[j];

828

if (manifold->getNumContacts()>0)

829

return false;

830

}

831

}

832

}

833

#endif

834

return true;

835

}

836

837

return false;

838

}

839

840

841

};

842

843

///internal debugging variable. this value shouldn't be too high

844

int gNumClampedCcdMotions=0;

845

846

void btDiscreteDynamicsWorld::integrateTransforms(btScalar timeStep)

847

{

848

BT_PROFILE("integrateTransforms");

849

btTransform predictedTrans;

850

for ( int i=0;i<m_nonStaticRigidBodies.size();i++)

851

{

852

btRigidBody* body = m_nonStaticRigidBodies[i];

853

body->setHitFraction(1.f);

854

855

if (body->isActive() && (!body->isStaticOrKinematicObject()))

856

{

857

858

body->predictIntegratedTransform(timeStep, predictedTrans);

859

860

btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();

861

862

863

864

if (getDispatchInfo().m_useContinuous && body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)

865

{

866

BT_PROFILE("CCD motion clamping");

867

if (body->getCollisionShape()->isConvex())

868

{

869

gNumClampedCcdMotions++;

870

#ifdef USE_STATIC_ONLY

871

class StaticOnlyCallback : public btClosestNotMeConvexResultCallback

872

{

873

public:

874

875

StaticOnlyCallback (btCollisionObject* me,const btVector3& fromA,const btVector3& toA,btOverlappingPairCache* pairCache,btDispatcher* dispatcher) :

876

btClosestNotMeConvexResultCallback(me,fromA,toA,pairCache,dispatcher)

877

{

878

}

879

880

virtual bool needsCollision(btBroadphaseProxy* proxy0) const

881

{

882

btCollisionObject* otherObj = (btCollisionObject*) proxy0->m_clientObject;

883

if (!otherObj->isStaticOrKinematicObject())

884

return false;

885

return btClosestNotMeConvexResultCallback::needsCollision(proxy0);

886

}

887

};

888

889

StaticOnlyCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());

890

#else

891

btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());

892

#endif

893

//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());

894

btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());

895

sweepResults.m_allowedPenetration=getDispatchInfo().m_allowedCcdPenetration;

896

897

sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;

898

sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;

899

btTransform modifiedPredictedTrans = predictedTrans;

900

modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());

901

902

convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);

903

if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))

904

{

905

906

//printf("clamped integration to hit fraction = %f\n",fraction);

907

body->setHitFraction(sweepResults.m_closestHitFraction);

908

body->predictIntegratedTransform(timeStep*body->getHitFraction(), predictedTrans);

909

body->setHitFraction(0.f);

910

body->proceedToTransform( predictedTrans);

911

912

#if 0

913

btVector3 linVel = body->getLinearVelocity();

914

915

btScalar maxSpeed = body->getCcdMotionThreshold()/getSolverInfo().m_timeStep;

916

btScalar maxSpeedSqr = maxSpeed*maxSpeed;

917

if (linVel.length2()>maxSpeedSqr)

918

{

919

linVel.normalize();

920

linVel*= maxSpeed;

921

body->setLinearVelocity(linVel);

922

btScalar ms2 = body->getLinearVelocity().length2();

923

body->predictIntegratedTransform(timeStep, predictedTrans);

924

925

btScalar sm2 = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();

926

btScalar smt = body->getCcdSquareMotionThreshold();

927

printf("sm2=%f\n",sm2);

928

}

929

#else

930

//response between two dynamic objects without friction, assuming 0 penetration depth

931

btScalar appliedImpulse = 0.f;

932

btScalar depth = 0.f;

933

appliedImpulse = resolveSingleCollision(body,sweepResults.m_hitCollisionObject,sweepResults.m_hitPointWorld,sweepResults.m_hitNormalWorld,getSolverInfo(), depth);

934

935

936

#endif

937

938

continue;

939

}

940

}

941

}

942

943

944

body->proceedToTransform( predictedTrans);

945

}

946

}

947

}

948

949

void btDiscreteDynamicsWorld::addSpeculativeContacts(btScalar timeStep)

950

{

951

BT_PROFILE("addSpeculativeContacts");

952

btTransform predictedTrans;

953

for ( int i=0;i<m_nonStaticRigidBodies.size();i++)

954

{

955

btRigidBody* body = m_nonStaticRigidBodies[i];

956

body->setHitFraction(1.f);

957

958

if (body->isActive() && (!body->isStaticOrKinematicObject()))

959

{

960

body->predictIntegratedTransform(timeStep, predictedTrans);

961

btScalar squareMotion = (predictedTrans.getOrigin()-body->getWorldTransform().getOrigin()).length2();

962

963

if (body->getCcdSquareMotionThreshold() && body->getCcdSquareMotionThreshold() < squareMotion)

964

{

965

BT_PROFILE("search speculative contacts");

966

if (body->getCollisionShape()->isConvex())

967

{

968

gNumClampedCcdMotions++;

969

970

btClosestNotMeConvexResultCallback sweepResults(body,body->getWorldTransform().getOrigin(),predictedTrans.getOrigin(),getBroadphase()->getOverlappingPairCache(),getDispatcher());

971

//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());

972

btSphereShape tmpSphere(body->getCcdSweptSphereRadius());//btConvexShape* convexShape = static_cast<btConvexShape*>(body->getCollisionShape());

973

974

sweepResults.m_collisionFilterGroup = body->getBroadphaseProxy()->m_collisionFilterGroup;

975

sweepResults.m_collisionFilterMask = body->getBroadphaseProxy()->m_collisionFilterMask;

976

btTransform modifiedPredictedTrans;

977

modifiedPredictedTrans = predictedTrans;

978

modifiedPredictedTrans.setBasis(body->getWorldTransform().getBasis());

979

980

convexSweepTest(&tmpSphere,body->getWorldTransform(),modifiedPredictedTrans,sweepResults);

981

if (sweepResults.hasHit() && (sweepResults.m_closestHitFraction < 1.f))

982

{

983

btBroadphaseProxy* proxy0 = body->getBroadphaseHandle();

984

btBroadphaseProxy* proxy1 = sweepResults.m_hitCollisionObject->getBroadphaseHandle();

985

btBroadphasePair* pair = sweepResults.m_pairCache->findPair(proxy0,proxy1);

986

if (pair)

987

{

988

if (pair->m_algorithm)

989

{

990

btManifoldArray contacts;

991

pair->m_algorithm->getAllContactManifolds(contacts);

992

if (contacts.size())

993

{

994

btManifoldResult result(body,sweepResults.m_hitCollisionObject);

995

result.setPersistentManifold(contacts[0]);

996

997

btVector3 vec = (modifiedPredictedTrans.getOrigin()-body->getWorldTransform().getOrigin());

998

vec*=sweepResults.m_closestHitFraction;

999

1000

btScalar lenSqr = vec.length2();

1001

btScalar depth = 0.f;

1002

btVector3 pointWorld = sweepResults.m_hitPointWorld;

1003

if (lenSqr>SIMD_EPSILON)

1004

{

1005

depth = btSqrt(lenSqr);

1006

pointWorld -= vec;

1007

vec /= depth;

1008

}

1009

1010

if (contacts[0]->getBody0()==body)

1011

{

1012

result.addContactPoint(sweepResults.m_hitNormalWorld,pointWorld,depth);

1013

#if 0

1014

debugContacts.push_back(sweepResults.m_hitPointWorld);//sweepResults.m_hitPointWorld);

1015

debugNormals.push_back(sweepResults.m_hitNormalWorld);

1016

#endif

1017

} else

1018

{

1019

//swapped

1020

result.addContactPoint(-sweepResults.m_hitNormalWorld,pointWorld,depth);

1021

//sweepResults.m_hitPointWorld,depth);

1022

1023

#if 0

1024

if (1)//firstHit==1)

1025

{

1026

firstHit=0;

1027

debugNormals.push_back(sweepResults.m_hitNormalWorld);

1028

debugContacts.push_back(pointWorld);//sweepResults.m_hitPointWorld);

1029

debugNormals.push_back(sweepResults.m_hitNormalWorld);

1030

debugContacts.push_back(sweepResults.m_hitPointWorld);

1031

}

1032

firstHit--;

1033

#endif

1034

}

1035

}

1036

1037

} else

1038

{

1039

//no algorithm, use dispatcher to create one

1040

1041

}

1042

1043

1044

} else

1045

{

1046

//add an overlapping pair

1047

//printf("pair missing\n");

1048

1049

}

1050

}

1051

}

1052

}

1053

1054

}

1055

}

1056

}

1057

1058

1059

1060

1061

1062

void btDiscreteDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)

1063

{

1064

BT_PROFILE("predictUnconstraintMotion");

1065

for ( int i=0;i<m_nonStaticRigidBodies.size();i++)

1066

{

1067

btRigidBody* body = m_nonStaticRigidBodies[i];

1068

if (!body->isStaticOrKinematicObject())

1069

{

1070

body->integrateVelocities( timeStep);

1071

//damping

1072

body->applyDamping(timeStep);

1073

1074

body->predictIntegratedTransform(timeStep,body->getInterpolationWorldTransform());

1075

}

1076

}

1077

}

1078

1079

1080

void btDiscreteDynamicsWorld::startProfiling(btScalar timeStep)

1081

{

1082

(void)timeStep;

1083

1084

#ifndef BT_NO_PROFILE

1085

CProfileManager::Reset();

1086

#endif //BT_NO_PROFILE

1087

1088

}

1089

1090

1091

1092

1093

1094

1095

void btDiscreteDynamicsWorld::debugDrawConstraint(btTypedConstraint* constraint)

1096

{

1097

bool drawFrames = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraints) != 0;

1098

bool drawLimits = (getDebugDrawer()->getDebugMode() & btIDebugDraw::DBG_DrawConstraintLimits) != 0;

1099

btScalar dbgDrawSize = constraint->getDbgDrawSize();

1100

if(dbgDrawSize <= btScalar(0.f))

1101

{

1102

return;

1103

}

1104

1105

switch(constraint->getConstraintType())

1106

{

1107

case POINT2POINT_CONSTRAINT_TYPE:

1108

{

1109

btPoint2PointConstraint* p2pC = (btPoint2PointConstraint*)constraint;

1110

btTransform tr;

1111

tr.setIdentity();

1112

btVector3 pivot = p2pC->getPivotInA();

1113

pivot = p2pC->getRigidBodyA().getCenterOfMassTransform() * pivot;

1114

tr.setOrigin(pivot);

1115

getDebugDrawer()->drawTransform(tr, dbgDrawSize);

1116

// that ideally should draw the same frame

1117

pivot = p2pC->getPivotInB();

1118

pivot = p2pC->getRigidBodyB().getCenterOfMassTransform() * pivot;

1119

tr.setOrigin(pivot);

1120

if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);

1121

}

1122

break;

1123

case HINGE_CONSTRAINT_TYPE:

1124

{

1125

btHingeConstraint* pHinge = (btHingeConstraint*)constraint;

1126

btTransform tr = pHinge->getRigidBodyA().getCenterOfMassTransform() * pHinge->getAFrame();

1127

if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);

1128

tr = pHinge->getRigidBodyB().getCenterOfMassTransform() * pHinge->getBFrame();

1129

if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);

1130

btScalar minAng = pHinge->getLowerLimit();

1131

btScalar maxAng = pHinge->getUpperLimit();

1132

if(minAng == maxAng)

1133

{

1134

break;

1135

}

1136

bool drawSect = true;

1137

if(minAng > maxAng)

1138

{

1139

minAng = btScalar(0.f);

1140

maxAng = SIMD_2_PI;

1141

drawSect = false;

1142

}

1143

if(drawLimits)

1144

{

1145

btVector3& center = tr.getOrigin();

1146

btVector3 normal = tr.getBasis().getColumn(2);

1147

btVector3 axis = tr.getBasis().getColumn(0);

1148

getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, minAng, maxAng, btVector3(0,0,0), drawSect);

1149

}

1150

}

1151

break;

1152

case CONETWIST_CONSTRAINT_TYPE:

1153

{

1154

btConeTwistConstraint* pCT = (btConeTwistConstraint*)constraint;

1155

btTransform tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();

1156

if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);

1157

tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();

1158

if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);

1159

if(drawLimits)

1160

{

1161

//const btScalar length = btScalar(5);

1162

const btScalar length = dbgDrawSize;

1163

static int nSegments = 8*4;

1164

btScalar fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)(nSegments-1)/btScalar(nSegments);

1165

btVector3 pPrev = pCT->GetPointForAngle(fAngleInRadians, length);

1166

pPrev = tr * pPrev;

1167

for (int i=0; i<nSegments; i++)

1168

{

1169

fAngleInRadians = btScalar(2.*3.1415926) * (btScalar)i/btScalar(nSegments);

1170

btVector3 pCur = pCT->GetPointForAngle(fAngleInRadians, length);

1171

pCur = tr * pCur;

1172

getDebugDrawer()->drawLine(pPrev, pCur, btVector3(0,0,0));

1173

1174

if (i%(nSegments/8) == 0)

1175

getDebugDrawer()->drawLine(tr.getOrigin(), pCur, btVector3(0,0,0));

1176

1177

pPrev = pCur;

1178

}

1179

btScalar tws = pCT->getTwistSpan();

1180

btScalar twa = pCT->getTwistAngle();

1181

bool useFrameB = (pCT->getRigidBodyB().getInvMass() > btScalar(0.f));

1182

if(useFrameB)

1183

{

1184

tr = pCT->getRigidBodyB().getCenterOfMassTransform() * pCT->getBFrame();

1185

}

1186

else

1187

{

1188

tr = pCT->getRigidBodyA().getCenterOfMassTransform() * pCT->getAFrame();

1189

}

1190

btVector3 pivot = tr.getOrigin();

1191

btVector3 normal = tr.getBasis().getColumn(0);

1192

btVector3 axis1 = tr.getBasis().getColumn(1);

1193

getDebugDrawer()->drawArc(pivot, normal, axis1, dbgDrawSize, dbgDrawSize, -twa-tws, -twa+tws, btVector3(0,0,0), true);

1194

1195

}

1196

}

1197

break;

1198

case D6_SPRING_CONSTRAINT_TYPE:

1199

case D6_CONSTRAINT_TYPE:

1200

{

1201

btGeneric6DofConstraint* p6DOF = (btGeneric6DofConstraint*)constraint;

1202

btTransform tr = p6DOF->getCalculatedTransformA();

1203

if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);

1204

tr = p6DOF->getCalculatedTransformB();

1205

if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);

1206

if(drawLimits)

1207

{

1208

tr = p6DOF->getCalculatedTransformA();

1209

const btVector3& center = p6DOF->getCalculatedTransformB().getOrigin();

1210

btVector3 up = tr.getBasis().getColumn(2);

1211

btVector3 axis = tr.getBasis().getColumn(0);

1212

btScalar minTh = p6DOF->getRotationalLimitMotor(1)->m_loLimit;

1213

btScalar maxTh = p6DOF->getRotationalLimitMotor(1)->m_hiLimit;

1214

btScalar minPs = p6DOF->getRotationalLimitMotor(2)->m_loLimit;

1215

btScalar maxPs = p6DOF->getRotationalLimitMotor(2)->m_hiLimit;

1216

getDebugDrawer()->drawSpherePatch(center, up, axis, dbgDrawSize * btScalar(.9f), minTh, maxTh, minPs, maxPs, btVector3(0,0,0));

1217

axis = tr.getBasis().getColumn(1);

1218

btScalar ay = p6DOF->getAngle(1);

1219

btScalar az = p6DOF->getAngle(2);

1220

btScalar cy = btCos(ay);

1221

btScalar sy = btSin(ay);

1222

btScalar cz = btCos(az);

1223

btScalar sz = btSin(az);

1224

btVector3 ref;

1225

ref[0] = cy*cz*axis[0] + cy*sz*axis[1] - sy*axis[2];

1226

ref[1] = -sz*axis[0] + cz*axis[1];

1227

ref[2] = cz*sy*axis[0] + sz*sy*axis[1] + cy*axis[2];

1228

tr = p6DOF->getCalculatedTransformB();

1229

btVector3 normal = -tr.getBasis().getColumn(0);

1230

btScalar minFi = p6DOF->getRotationalLimitMotor(0)->m_loLimit;

1231

btScalar maxFi = p6DOF->getRotationalLimitMotor(0)->m_hiLimit;

1232

if(minFi > maxFi)

1233

{

1234

getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, -SIMD_PI, SIMD_PI, btVector3(0,0,0), false);

1235

}

1236

else if(minFi < maxFi)

1237

{

1238

getDebugDrawer()->drawArc(center, normal, ref, dbgDrawSize, dbgDrawSize, minFi, maxFi, btVector3(0,0,0), true);

1239

}

1240

tr = p6DOF->getCalculatedTransformA();

1241

btVector3 bbMin = p6DOF->getTranslationalLimitMotor()->m_lowerLimit;

1242

btVector3 bbMax = p6DOF->getTranslationalLimitMotor()->m_upperLimit;

1243

getDebugDrawer()->drawBox(bbMin, bbMax, tr, btVector3(0,0,0));

1244

}

1245

}

1246

break;

1247

case SLIDER_CONSTRAINT_TYPE:

1248

{

1249

btSliderConstraint* pSlider = (btSliderConstraint*)constraint;

1250

btTransform tr = pSlider->getCalculatedTransformA();

1251

if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);

1252

tr = pSlider->getCalculatedTransformB();

1253

if(drawFrames) getDebugDrawer()->drawTransform(tr, dbgDrawSize);

1254

if(drawLimits)

1255

{

1256

btTransform tr = pSlider->getUseLinearReferenceFrameA() ? pSlider->getCalculatedTransformA() : pSlider->getCalculatedTransformB();

1257

btVector3 li_min = tr * btVector3(pSlider->getLowerLinLimit(), 0.f, 0.f);

1258

btVector3 li_max = tr * btVector3(pSlider->getUpperLinLimit(), 0.f, 0.f);

1259

getDebugDrawer()->drawLine(li_min, li_max, btVector3(0, 0, 0));

1260

btVector3 normal = tr.getBasis().getColumn(0);

1261

btVector3 axis = tr.getBasis().getColumn(1);

1262

btScalar a_min = pSlider->getLowerAngLimit();

1263

btScalar a_max = pSlider->getUpperAngLimit();

1264

const btVector3& center = pSlider->getCalculatedTransformB().getOrigin();

1265

getDebugDrawer()->drawArc(center, normal, axis, dbgDrawSize, dbgDrawSize, a_min, a_max, btVector3(0,0,0), true);

1266

}

1267

}

1268

break;

1269

default :

1270

break;

1271

}

1272

return;

1273

}

1274

1275

1276

1277

1278

1279

void btDiscreteDynamicsWorld::setConstraintSolver(btConstraintSolver* solver)

1280

{

1281

if (m_ownsConstraintSolver)

1282

{

1283

btAlignedFree( m_constraintSolver);

1284

}

1285

m_ownsConstraintSolver = false;

1286

m_constraintSolver = solver;

1287

}

1288

1289

btConstraintSolver* btDiscreteDynamicsWorld::getConstraintSolver()

1290

{

1291

return m_constraintSolver;

1292

}

1293

1294

1295

int btDiscreteDynamicsWorld::getNumConstraints() const

1296

{

1297

return int(m_constraints.size());

1298

}

1299

btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index)

1300

{

1301

return m_constraints[index];

1302

}

1303

const btTypedConstraint* btDiscreteDynamicsWorld::getConstraint(int index) const

1304

{

1305

return m_constraints[index];

1306

}

1307

1308

1309

1310

void btDiscreteDynamicsWorld::serializeRigidBodies(btSerializer* serializer)

1311

{

1312

int i;

1313

//serialize all collision objects

1314

for (i=0;i<m_collisionObjects.size();i++)

1315

{

1316

btCollisionObject* colObj = m_collisionObjects[i];

1317

if (colObj->getInternalType() & btCollisionObject::CO_RIGID_BODY)

1318

{

1319

int len = colObj->calculateSerializeBufferSize();

1320

btChunk* chunk = serializer->allocate(len,1);

1321

const char* structType = colObj->serialize(chunk->m_oldPtr, serializer);

1322

serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,colObj);

1323

}

1324

}

1325

1326

for (i=0;i<m_constraints.size();i++)

1327

{

1328

btTypedConstraint* constraint = m_constraints[i];

1329

int size = constraint->calculateSerializeBufferSize();

1330

btChunk* chunk = serializer->allocate(size,1);

1331

const char* structType = constraint->serialize(chunk->m_oldPtr,serializer);

1332

serializer->finalizeChunk(chunk,structType,BT_CONSTRAINT_CODE,constraint);

1333

}

1334

}

1335

1336

1337

void btDiscreteDynamicsWorld::serialize(btSerializer* serializer)

1338

{

1339

1340

serializer->startSerialization();

1341

1342

serializeRigidBodies(serializer);

1343

1344

serializeCollisionObjects(serializer);

1345

1346

serializer->finishSerialization();

1347

}

1348

Older »