~ubuntu-branches/ubuntu/saucy/emscripten/saucy-proposed

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 "SpuGatheringCollisionTask.h"

//#define DEBUG_SPU_COLLISION_DETECTION 1

#include "../SpuDoubleBuffer.h"

#include "../SpuCollisionTaskProcess.h"

#include "../SpuGatheringCollisionDispatcher.h" //for SPU_BATCHSIZE_BROADPHASE_PAIRS

#include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"

#include "../SpuContactManifoldCollisionAlgorithm.h"

#include "BulletCollision/CollisionDispatch/btCollisionObject.h"

#include "SpuContactResult.h"

#include "BulletCollision/CollisionShapes/btOptimizedBvh.h"

#include "BulletCollision/CollisionShapes/btTriangleIndexVertexArray.h"

#include "BulletCollision/CollisionShapes/btSphereShape.h"

#include "BulletCollision/CollisionShapes/btConvexPointCloudShape.h"

#include "BulletCollision/CollisionShapes/btCapsuleShape.h"

#include "BulletCollision/CollisionShapes/btConvexShape.h"

#include "BulletCollision/CollisionShapes/btBvhTriangleMeshShape.h"

#include "BulletCollision/CollisionShapes/btConvexHullShape.h"

#include "BulletCollision/CollisionShapes/btCompoundShape.h"

#include "SpuMinkowskiPenetrationDepthSolver.h"

//#include "SpuEpaPenetrationDepthSolver.h"

#include "BulletCollision/NarrowPhaseCollision/btGjkPairDetector.h"

#include "boxBoxDistance.h"

#include "BulletMultiThreaded/vectormath2bullet.h"

#include "SpuCollisionShapes.h" //definition of SpuConvexPolyhedronVertexData

#include "BulletCollision/CollisionDispatch/btBoxBoxDetector.h"

#include "BulletCollision/NarrowPhaseCollision/btGjkEpaPenetrationDepthSolver.h"

#include "BulletCollision/CollisionShapes/btTriangleShape.h"

#ifdef __SPU__

///Software caching from the IBM Cell SDK, it reduces 25% SPU time for our test cases

#ifndef USE_LIBSPE2

//#define USE_SOFTWARE_CACHE 1

#endif

#endif //__SPU__

int gSkippedCol = 0;

int gProcessedCol = 0;

////////////////////////////////////////////////

/// software caching

#if USE_SOFTWARE_CACHE

#include <spu_intrinsics.h>

#include <sys/spu_thread.h>

#include <sys/spu_event.h>

#include <stdint.h>

#define SPE_CACHE_NWAY 4

//#define SPE_CACHE_NSETS 32, 16

#define SPE_CACHE_NSETS 8

//#define SPE_CACHELINE_SIZE 512

#define SPE_CACHELINE_SIZE 128

#define SPE_CACHE_SET_TAGID(set) 15

///make sure that spe_cache.h is below those defines!

#include "../Extras/software_cache/cache/include/spe_cache.h"

int g_CacheMisses=0;

int g_CacheHits=0;

#if 0 // Added to allow cache misses and hits to be tracked, change this to 1 to restore unmodified version

#define spe_cache_read(ea) _spe_cache_lookup_xfer_wait_(ea, 0, 1)

#else

#define spe_cache_read(ea) \

({ \

int set, idx, line, byte; \

_spe_cache_nway_lookup_(ea, set, idx); \

if (btUnlikely(idx < 0)) { \

++g_CacheMisses; \

idx = _spe_cache_miss_(ea, set, -1); \

spu_writech(22, SPE_CACHE_SET_TAGMASK(set)); \

spu_mfcstat(MFC_TAG_UPDATE_ALL); \

} \

else \

{ \

++g_CacheHits; \

} \

100

line = _spe_cacheline_num_(set, idx); \

101

byte = _spe_cacheline_byte_offset_(ea); \

102

(void *) &spe_cache_mem[line + byte]; \

103

})

104

105

#endif

106

107

#endif // USE_SOFTWARE_CACHE

108

109

bool gUseEpa = false;

110

111

#ifdef USE_SN_TUNER

112

#include <LibSN_SPU.h>

113

#endif //USE_SN_TUNER

114

115

#if defined (__SPU__) && !defined (USE_LIBSPE2)

116

#include <spu_printf.h>

117

#elif defined (USE_LIBSPE2)

118

#define spu_printf(a)

119

#else

120

#define IGNORE_ALIGNMENT 1

121

#include <stdio.h>

122

#include <stdlib.h>

123

#define spu_printf printf

124

125

#endif

126

127

//int gNumConvexPoints0=0;

128

129

///Make sure no destructors are called on this memory

130

struct CollisionTask_LocalStoreMemory

131

{

132

///This CollisionTask_LocalStoreMemory is mainly used for the SPU version, using explicit DMA

133

///Other platforms can use other memory programming models.

134

135

ATTRIBUTE_ALIGNED16(btBroadphasePair gBroadphasePairsBuffer[SPU_BATCHSIZE_BROADPHASE_PAIRS]);

136

DoubleBuffer<unsigned char, MIDPHASE_WORKUNIT_PAGE_SIZE> g_workUnitTaskBuffers;

137

ATTRIBUTE_ALIGNED16(char gSpuContactManifoldAlgoBuffer [sizeof(SpuContactManifoldCollisionAlgorithm)+16]);

138

ATTRIBUTE_ALIGNED16(char gColObj0Buffer [sizeof(btCollisionObject)+16]);

139

ATTRIBUTE_ALIGNED16(char gColObj1Buffer [sizeof(btCollisionObject)+16]);

140

///we reserve 32bit integer indices, even though they might be 16bit

141

ATTRIBUTE_ALIGNED16(int spuIndices[16]);

142

btPersistentManifold gPersistentManifoldBuffer;

143

CollisionShape_LocalStoreMemory gCollisionShapes[2];

144

bvhMeshShape_LocalStoreMemory bvhShapeData;

145

SpuConvexPolyhedronVertexData convexVertexData[2];

146

CompoundShape_LocalStoreMemory compoundShapeData[2];

147

148

///The following pointers might either point into this local store memory, or to the original/other memory locations.

149

///See SpuFakeDma for implementation of cellDmaSmallGetReadOnly.

150

btCollisionObject* m_lsColObj0Ptr;

151

btCollisionObject* m_lsColObj1Ptr;

152

btBroadphasePair* m_pairsPointer;

153

btPersistentManifold* m_lsManifoldPtr;

154

SpuContactManifoldCollisionAlgorithm* m_lsCollisionAlgorithmPtr;

155

156

bool needsDmaPutContactManifoldAlgo;

157

158

btCollisionObject* getColObj0()

159

{

160

return m_lsColObj0Ptr;

161

}

162

btCollisionObject* getColObj1()

163

{

164

return m_lsColObj1Ptr;

165

}

166

167

168

btBroadphasePair* getBroadphasePairPtr()

169

{

170

return m_pairsPointer;

171

}

172

173

SpuContactManifoldCollisionAlgorithm* getlocalCollisionAlgorithm()

174

{

175

return m_lsCollisionAlgorithmPtr;

176

}

177

178

btPersistentManifold* getContactManifoldPtr()

179

{

180

return m_lsManifoldPtr;

181

}

182

};

183

184

185

#if defined(__CELLOS_LV2__) || defined(USE_LIBSPE2)

186

187

ATTRIBUTE_ALIGNED16(CollisionTask_LocalStoreMemory gLocalStoreMemory);

188

189

void* createCollisionLocalStoreMemory()

190

{

191

return &gLocalStoreMemory;

192

}

193

#else

194

void* createCollisionLocalStoreMemory()

195

{

196

return new CollisionTask_LocalStoreMemory;

197

}

198

199

#endif

200

201

void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts);

202

203

204

SIMD_FORCE_INLINE void small_cache_read(void* buffer, ppu_address_t ea, size_t size)

205

{

206

#if USE_SOFTWARE_CACHE

207

// Check for alignment requirements. We need to make sure the entire request fits within one cache line,

208

// so the first and last bytes should fall on the same cache line

209

btAssert((ea & ~SPE_CACHELINE_MASK) == ((ea + size - 1) & ~SPE_CACHELINE_MASK));

210

211

void* ls = spe_cache_read(ea);

212

memcpy(buffer, ls, size);

213

#else

214

stallingUnalignedDmaSmallGet(buffer,ea,size);

215

#endif

216

}

217

218

SIMD_FORCE_INLINE void small_cache_read_triple( void* ls0, ppu_address_t ea0,

219

void* ls1, ppu_address_t ea1,

220

void* ls2, ppu_address_t ea2,

221

size_t size)

222

{

223

btAssert(size<16);

224

ATTRIBUTE_ALIGNED16(char tmpBuffer0[32]);

225

ATTRIBUTE_ALIGNED16(char tmpBuffer1[32]);

226

ATTRIBUTE_ALIGNED16(char tmpBuffer2[32]);

227

228

uint32_t i;

229

230

231

///make sure last 4 bits are the same, for cellDmaSmallGet

232

char* localStore0 = (char*)ls0;

233

uint32_t last4BitsOffset = ea0 & 0x0f;

234

char* tmpTarget0 = tmpBuffer0 + last4BitsOffset;

235

#ifdef __SPU__

236

cellDmaSmallGet(tmpTarget0,ea0,size,DMA_TAG(1),0,0);

237

#else

238

tmpTarget0 = (char*)cellDmaSmallGetReadOnly(tmpTarget0,ea0,size,DMA_TAG(1),0,0);

239

#endif

240

241

242

char* localStore1 = (char*)ls1;

243

last4BitsOffset = ea1 & 0x0f;

244

char* tmpTarget1 = tmpBuffer1 + last4BitsOffset;

245

#ifdef __SPU__

246

cellDmaSmallGet(tmpTarget1,ea1,size,DMA_TAG(1),0,0);

247

#else

248

tmpTarget1 = (char*)cellDmaSmallGetReadOnly(tmpTarget1,ea1,size,DMA_TAG(1),0,0);

249

#endif

250

251

char* localStore2 = (char*)ls2;

252

last4BitsOffset = ea2 & 0x0f;

253

char* tmpTarget2 = tmpBuffer2 + last4BitsOffset;

254

#ifdef __SPU__

255

cellDmaSmallGet(tmpTarget2,ea2,size,DMA_TAG(1),0,0);

256

#else

257

tmpTarget2 = (char*)cellDmaSmallGetReadOnly(tmpTarget2,ea2,size,DMA_TAG(1),0,0);

258

#endif

259

260

261

cellDmaWaitTagStatusAll( DMA_MASK(1) );

262

263

//this is slowish, perhaps memcpy on SPU is smarter?

264

for (i=0; btLikely( i<size );i++)

265

{

266

localStore0[i] = tmpTarget0[i];

267

localStore1[i] = tmpTarget1[i];

268

localStore2[i] = tmpTarget2[i];

269

}

270

271

272

}

273

274

275

276

277

class spuNodeCallback : public btNodeOverlapCallback

278

{

279

SpuCollisionPairInput* m_wuInput;

280

SpuContactResult& m_spuContacts;

281

CollisionTask_LocalStoreMemory* m_lsMemPtr;

282

ATTRIBUTE_ALIGNED16(btTriangleShape) m_tmpTriangleShape;

283

284

ATTRIBUTE_ALIGNED16(btVector3 spuTriangleVertices[3]);

285

ATTRIBUTE_ALIGNED16(btScalar spuUnscaledVertex[4]);

286

287

288

289

public:

290

spuNodeCallback(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr,SpuContactResult& spuContacts)

291

: m_wuInput(wuInput),

292

m_spuContacts(spuContacts),

293

m_lsMemPtr(lsMemPtr)

294

{

295

}

296

297

virtual void processNode(int subPart, int triangleIndex)

298

{

299

///Create a triangle on the stack, call process collision, with GJK

300

///DMA the vertices, can benefit from software caching

301

302

// spu_printf("processNode with triangleIndex %d\n",triangleIndex);

303

304

if (m_lsMemPtr->bvhShapeData.gIndexMesh.m_indexType == PHY_SHORT)

305

{

306

unsigned short int* indexBasePtr = (unsigned short int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride);

307

ATTRIBUTE_ALIGNED16(unsigned short int tmpIndices[3]);

308

309

small_cache_read_triple(&tmpIndices[0],(ppu_address_t)&indexBasePtr[0],

310

&tmpIndices[1],(ppu_address_t)&indexBasePtr[1],

311

&tmpIndices[2],(ppu_address_t)&indexBasePtr[2],

312

sizeof(unsigned short int));

313

314

m_lsMemPtr->spuIndices[0] = int(tmpIndices[0]);

315

m_lsMemPtr->spuIndices[1] = int(tmpIndices[1]);

316

m_lsMemPtr->spuIndices[2] = int(tmpIndices[2]);

317

} else

318

{

319

unsigned int* indexBasePtr = (unsigned int*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexBase+triangleIndex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_triangleIndexStride);

320

321

small_cache_read_triple(&m_lsMemPtr->spuIndices[0],(ppu_address_t)&indexBasePtr[0],

322

&m_lsMemPtr->spuIndices[1],(ppu_address_t)&indexBasePtr[1],

323

&m_lsMemPtr->spuIndices[2],(ppu_address_t)&indexBasePtr[2],

324

sizeof(int));

325

}

326

327

// spu_printf("SPU index0=%d ,",spuIndices[0]);

328

// spu_printf("SPU index1=%d ,",spuIndices[1]);

329

// spu_printf("SPU index2=%d ,",spuIndices[2]);

330

// spu_printf("SPU: indexBasePtr=%llx\n",indexBasePtr);

331

332

const btVector3& meshScaling = m_lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getScaling();

333

for (int j=2;btLikely( j>=0 );j--)

334

{

335

int graphicsindex = m_lsMemPtr->spuIndices[j];

336

337

// spu_printf("SPU index=%d ,",graphicsindex);

338

btScalar* graphicsbasePtr = (btScalar*)(m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexBase+graphicsindex*m_lsMemPtr->bvhShapeData.gIndexMesh.m_vertexStride);

339

// spu_printf("SPU graphicsbasePtr=%llx\n",graphicsbasePtr);

340

341

342

///handle un-aligned vertices...

343

344

//another DMA for each vertex

345

small_cache_read_triple(&spuUnscaledVertex[0],(ppu_address_t)&graphicsbasePtr[0],

346

&spuUnscaledVertex[1],(ppu_address_t)&graphicsbasePtr[1],

347

&spuUnscaledVertex[2],(ppu_address_t)&graphicsbasePtr[2],

348

sizeof(btScalar));

349

350

m_tmpTriangleShape.getVertexPtr(j).setValue(spuUnscaledVertex[0]*meshScaling.getX(),

351

spuUnscaledVertex[1]*meshScaling.getY(),

352

spuUnscaledVertex[2]*meshScaling.getZ());

353

354

// spu_printf("SPU:triangle vertices:%f,%f,%f\n",spuTriangleVertices[j].x(),spuTriangleVertices[j].y(),spuTriangleVertices[j].z());

355

}

356

357

358

SpuCollisionPairInput triangleConcaveInput(*m_wuInput);

359

// triangleConcaveInput.m_spuCollisionShapes[1] = &spuTriangleVertices[0];

360

triangleConcaveInput.m_spuCollisionShapes[1] = &m_tmpTriangleShape;

361

triangleConcaveInput.m_shapeType1 = TRIANGLE_SHAPE_PROXYTYPE;

362

363

m_spuContacts.setShapeIdentifiersB(subPart,triangleIndex);

364

365

// m_spuContacts.flush();

366

367

ProcessSpuConvexConvexCollision(&triangleConcaveInput, m_lsMemPtr,m_spuContacts);

368

///this flush should be automatic

369

// m_spuContacts.flush();

370

}

371

372

};

373

374

375

376

void btConvexPlaneCollideSingleContact (SpuCollisionPairInput* wuInput,CollisionTask_LocalStoreMemory* lsMemPtr,SpuContactResult& spuContacts)

377

{

378

379

btConvexShape* convexShape = (btConvexShape*) wuInput->m_spuCollisionShapes[0];

380

btStaticPlaneShape* planeShape = (btStaticPlaneShape*) wuInput->m_spuCollisionShapes[1];

381

382

bool hasCollision = false;

383

const btVector3& planeNormal = planeShape->getPlaneNormal();

384

const btScalar& planeConstant = planeShape->getPlaneConstant();

385

386

387

btTransform convexWorldTransform = wuInput->m_worldTransform0;

388

btTransform convexInPlaneTrans;

389

convexInPlaneTrans= wuInput->m_worldTransform1.inverse() * convexWorldTransform;

390

btTransform planeInConvex;

391

planeInConvex= convexWorldTransform.inverse() * wuInput->m_worldTransform1;

392

393

//btVector3 vtx = convexShape->localGetSupportVertexWithoutMarginNonVirtual(planeInConvex.getBasis()*-planeNormal);

394

btVector3 vtx = convexShape->localGetSupportVertexNonVirtual(planeInConvex.getBasis()*-planeNormal);

395

396

btVector3 vtxInPlane = convexInPlaneTrans(vtx);

397

btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);

398

399

btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;

400

btVector3 vtxInPlaneWorld = wuInput->m_worldTransform1 * vtxInPlaneProjected;

401

402

hasCollision = distance < lsMemPtr->getContactManifoldPtr()->getContactBreakingThreshold();

403

//resultOut->setPersistentManifold(m_manifoldPtr);

404

if (hasCollision)

405

{

406

/// report a contact. internally this will be kept persistent, and contact reduction is done

407

btVector3 normalOnSurfaceB =wuInput->m_worldTransform1.getBasis() * planeNormal;

408

btVector3 pOnB = vtxInPlaneWorld;

409

spuContacts.addContactPoint(normalOnSurfaceB,pOnB,distance);

410

}

411

}

412

413

void ProcessConvexPlaneSpuCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts)

414

{

415

416

417

418

btPersistentManifold* manifold = (btPersistentManifold*)wuInput->m_persistentManifoldPtr;

419

420

///DMA in the vertices for convex shapes

421

ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]);

422

ATTRIBUTE_ALIGNED16(char convexHullShape1[sizeof(btConvexHullShape)]);

423

424

if ( btLikely( wuInput->m_shapeType0== CONVEX_HULL_SHAPE_PROXYTYPE ) )

425

{

426

// spu_printf("SPU: DMA btConvexHullShape\n");

427

428

dmaSize = sizeof(btConvexHullShape);

429

dmaPpuAddress2 = wuInput->m_collisionShapes[0];

430

431

cellDmaGet(&convexHullShape0, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);

432

//cellDmaWaitTagStatusAll(DMA_MASK(1));

433

}

434

435

if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )

436

{

437

// spu_printf("SPU: DMA btConvexHullShape\n");

438

dmaSize = sizeof(btConvexHullShape);

439

dmaPpuAddress2 = wuInput->m_collisionShapes[1];

440

cellDmaGet(&convexHullShape1, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);

441

//cellDmaWaitTagStatusAll(DMA_MASK(1));

442

}

443

444

if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) )

445

{

446

cellDmaWaitTagStatusAll(DMA_MASK(1));

447

dmaConvexVertexData (&lsMemPtr->convexVertexData[0], (btConvexHullShape*)&convexHullShape0);

448

lsMemPtr->convexVertexData[0].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[0];

449

}

450

451

452

if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )

453

{

454

cellDmaWaitTagStatusAll(DMA_MASK(1));

455

dmaConvexVertexData (&lsMemPtr->convexVertexData[1], (btConvexHullShape*)&convexHullShape1);

456

lsMemPtr->convexVertexData[1].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[1];

457

}

458

459

460

btConvexPointCloudShape cpc0,cpc1;

461

462

if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) )

463

{

464

cellDmaWaitTagStatusAll(DMA_MASK(2));

465

lsMemPtr->convexVertexData[0].gConvexPoints = &lsMemPtr->convexVertexData[0].g_convexPointBuffer[0];

466

btConvexHullShape* ch = (btConvexHullShape*)wuInput->m_spuCollisionShapes[0];

467

const btVector3& localScaling = ch->getLocalScalingNV();

468

cpc0.setPoints(lsMemPtr->convexVertexData[0].gConvexPoints,lsMemPtr->convexVertexData[0].gNumConvexPoints,false,localScaling);

469

wuInput->m_spuCollisionShapes[0] = &cpc0;

470

}

471

472

if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )

473

{

474

cellDmaWaitTagStatusAll(DMA_MASK(2));

475

lsMemPtr->convexVertexData[1].gConvexPoints = &lsMemPtr->convexVertexData[1].g_convexPointBuffer[0];

476

btConvexHullShape* ch = (btConvexHullShape*)wuInput->m_spuCollisionShapes[1];

477

const btVector3& localScaling = ch->getLocalScalingNV();

478

cpc1.setPoints(lsMemPtr->convexVertexData[1].gConvexPoints,lsMemPtr->convexVertexData[1].gNumConvexPoints,false,localScaling);

479

wuInput->m_spuCollisionShapes[1] = &cpc1;

480

481

}

482

483

484

// const btConvexShape* shape0Ptr = (const btConvexShape*)wuInput->m_spuCollisionShapes[0];

485

// const btConvexShape* shape1Ptr = (const btConvexShape*)wuInput->m_spuCollisionShapes[1];

486

// int shapeType0 = wuInput->m_shapeType0;

487

// int shapeType1 = wuInput->m_shapeType1;

488

float marginA = wuInput->m_collisionMargin0;

489

float marginB = wuInput->m_collisionMargin1;

490

491

SpuClosestPointInput cpInput;

492

cpInput.m_convexVertexData[0] = &lsMemPtr->convexVertexData[0];

493

cpInput.m_convexVertexData[1] = &lsMemPtr->convexVertexData[1];

494

cpInput.m_transformA = wuInput->m_worldTransform0;

495

cpInput.m_transformB = wuInput->m_worldTransform1;

496

float sumMargin = (marginA+marginB+lsMemPtr->getContactManifoldPtr()->getContactBreakingThreshold());

497

cpInput.m_maximumDistanceSquared = sumMargin * sumMargin;

498

499

ppu_address_t manifoldAddress = (ppu_address_t)manifold;

500

501

btPersistentManifold* spuManifold=lsMemPtr->getContactManifoldPtr();

502

//spuContacts.setContactInfo(spuManifold,manifoldAddress,wuInput->m_worldTransform0,wuInput->m_worldTransform1,wuInput->m_isSwapped);

503

spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMemPtr->getColObj0()->getWorldTransform(),

504

lsMemPtr->getColObj1()->getWorldTransform(),

505

lsMemPtr->getColObj0()->getRestitution(),lsMemPtr->getColObj1()->getRestitution(),

506

lsMemPtr->getColObj0()->getFriction(),lsMemPtr->getColObj1()->getFriction(),

507

wuInput->m_isSwapped);

508

509

510

btConvexPlaneCollideSingleContact(wuInput,lsMemPtr,spuContacts);

511

512

513

514

515

}

516

517

518

519

520

////////////////////////

521

/// Convex versus Concave triangle mesh collision detection (handles concave triangle mesh versus sphere, box, cylinder, triangle, cone, convex polyhedron etc)

522

///////////////////

523

void ProcessConvexConcaveSpuCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts)

524

{

525

//order: first collision shape is convex, second concave. m_isSwapped is true, if the original order was opposite

526

527

btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)wuInput->m_spuCollisionShapes[1];

528

//need the mesh interface, for access to triangle vertices

529

dmaBvhShapeData (&lsMemPtr->bvhShapeData, trimeshShape);

530

531

btVector3 aabbMin(-1,-400,-1);

532

btVector3 aabbMax(1,400,1);

533

534

535

//recalc aabbs

536

btTransform convexInTriangleSpace;

537

convexInTriangleSpace = wuInput->m_worldTransform1.inverse() * wuInput->m_worldTransform0;

538

btConvexInternalShape* convexShape = (btConvexInternalShape*)wuInput->m_spuCollisionShapes[0];

539

540

computeAabb (aabbMin, aabbMax, convexShape, wuInput->m_collisionShapes[0], wuInput->m_shapeType0, convexInTriangleSpace);

541

542

543

//CollisionShape* triangleShape = static_cast<btCollisionShape*>(triBody->m_collisionShape);

544

//convexShape->getAabb(convexInTriangleSpace,m_aabbMin,m_aabbMax);

545

546

// btScalar extraMargin = collisionMarginTriangle;

547

// btVector3 extra(extraMargin,extraMargin,extraMargin);

548

// aabbMax += extra;

549

// aabbMin -= extra;

550

551

///quantize query AABB

552

unsigned short int quantizedQueryAabbMin[3];

553

unsigned short int quantizedQueryAabbMax[3];

554

lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMin,aabbMin,0);

555

lsMemPtr->bvhShapeData.getOptimizedBvh()->quantizeWithClamp(quantizedQueryAabbMax,aabbMax,1);

556

557

QuantizedNodeArray& nodeArray = lsMemPtr->bvhShapeData.getOptimizedBvh()->getQuantizedNodeArray();

558

//spu_printf("SPU: numNodes = %d\n",nodeArray.size());

559

560

BvhSubtreeInfoArray& subTrees = lsMemPtr->bvhShapeData.getOptimizedBvh()->getSubtreeInfoArray();

561

562

563

spuNodeCallback nodeCallback(wuInput,lsMemPtr,spuContacts);

564

IndexedMeshArray& indexArray = lsMemPtr->bvhShapeData.gTriangleMeshInterfacePtr->getIndexedMeshArray();

565

//spu_printf("SPU:indexArray.size() = %d\n",indexArray.size());

566

567

// spu_printf("SPU: numSubTrees = %d\n",subTrees.size());

568

//not likely to happen

569

if (subTrees.size() && indexArray.size() == 1)

570

{

571

///DMA in the index info

572

dmaBvhIndexedMesh (&lsMemPtr->bvhShapeData.gIndexMesh, indexArray, 0 /* index into indexArray */, 1 /* dmaTag */);

573

cellDmaWaitTagStatusAll(DMA_MASK(1));

574

575

//display the headers

576

int numBatch = subTrees.size();

577

for (int i=0;i<numBatch;)

578

{

579

//@todo- can reorder DMA transfers for less stall

580

int remaining = subTrees.size() - i;

581

int nextBatch = remaining < MAX_SPU_SUBTREE_HEADERS ? remaining : MAX_SPU_SUBTREE_HEADERS;

582

583

dmaBvhSubTreeHeaders (&lsMemPtr->bvhShapeData.gSubtreeHeaders[0], (ppu_address_t)(&subTrees[i]), nextBatch, 1);

584

cellDmaWaitTagStatusAll(DMA_MASK(1));

585

586

587

// spu_printf("nextBatch = %d\n",nextBatch);

588

589

for (int j=0;j<nextBatch;j++)

590

{

591

const btBvhSubtreeInfo& subtree = lsMemPtr->bvhShapeData.gSubtreeHeaders[j];

592

593

unsigned int overlap = spuTestQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);

594

if (overlap)

595

{

596

btAssert(subtree.m_subtreeSize);

597

598

//dma the actual nodes of this subtree

599

dmaBvhSubTreeNodes (&lsMemPtr->bvhShapeData.gSubtreeNodes[0], subtree, nodeArray, 2);

600

cellDmaWaitTagStatusAll(DMA_MASK(2));

601

602

/* Walk this subtree */

603

spuWalkStacklessQuantizedTree(&nodeCallback,quantizedQueryAabbMin,quantizedQueryAabbMax,

604

&lsMemPtr->bvhShapeData.gSubtreeNodes[0],

605

606

subtree.m_subtreeSize);

607

}

608

// spu_printf("subtreeSize = %d\n",gSubtreeHeaders[j].m_subtreeSize);

609

}

610

611

// unsigned short int m_quantizedAabbMin[3];

612

// unsigned short int m_quantizedAabbMax[3];

613

// int m_rootNodeIndex;

614

// int m_subtreeSize;

615

i+=nextBatch;

616

}

617

618

//pre-fetch first tree, then loop and double buffer

619

}

620

621

}

622

623

624

int stats[11]={0,0,0,0,0,0,0,0,0,0,0};

625

int degenerateStats[11]={0,0,0,0,0,0,0,0,0,0,0};

626

627

628

////////////////////////

629

/// Convex versus Convex collision detection (handles collision between sphere, box, cylinder, triangle, cone, convex polyhedron etc)

630

///////////////////

631

void ProcessSpuConvexConvexCollision(SpuCollisionPairInput* wuInput, CollisionTask_LocalStoreMemory* lsMemPtr, SpuContactResult& spuContacts)

632

{

633

634

635

636

#ifdef DEBUG_SPU_COLLISION_DETECTION

637

//spu_printf("SPU: ProcessSpuConvexConvexCollision\n");

638

#endif //DEBUG_SPU_COLLISION_DETECTION

639

//CollisionShape* shape0 = (CollisionShape*)wuInput->m_collisionShapes[0];

640

//CollisionShape* shape1 = (CollisionShape*)wuInput->m_collisionShapes[1];

641

btPersistentManifold* manifold = (btPersistentManifold*)wuInput->m_persistentManifoldPtr;

642

643

bool genericGjk = true;

644

645

if (genericGjk)

646

{

647

//try generic GJK

648

649

650

651

//SpuConvexPenetrationDepthSolver* penetrationSolver=0;

652

btVoronoiSimplexSolver simplexSolver;

653

btGjkEpaPenetrationDepthSolver epaPenetrationSolver2;

654

655

btConvexPenetrationDepthSolver* penetrationSolver = &epaPenetrationSolver2;

656

657

//SpuMinkowskiPenetrationDepthSolver minkowskiPenetrationSolver;

658

#ifdef ENABLE_EPA

659

if (gUseEpa)

660

{

661

penetrationSolver = &epaPenetrationSolver2;

662

} else

663

#endif

664

{

665

//penetrationSolver = &minkowskiPenetrationSolver;

666

}

667

668

669

///DMA in the vertices for convex shapes

670

ATTRIBUTE_ALIGNED16(char convexHullShape0[sizeof(btConvexHullShape)]);

671

ATTRIBUTE_ALIGNED16(char convexHullShape1[sizeof(btConvexHullShape)]);

672

673

if ( btLikely( wuInput->m_shapeType0== CONVEX_HULL_SHAPE_PROXYTYPE ) )

674

{

675

// spu_printf("SPU: DMA btConvexHullShape\n");

676

677

dmaSize = sizeof(btConvexHullShape);

678

dmaPpuAddress2 = wuInput->m_collisionShapes[0];

679

680

cellDmaGet(&convexHullShape0, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);

681

//cellDmaWaitTagStatusAll(DMA_MASK(1));

682

}

683

684

if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )

685

{

686

// spu_printf("SPU: DMA btConvexHullShape\n");

687

dmaSize = sizeof(btConvexHullShape);

688

dmaPpuAddress2 = wuInput->m_collisionShapes[1];

689

cellDmaGet(&convexHullShape1, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);

690

//cellDmaWaitTagStatusAll(DMA_MASK(1));

691

}

692

693

if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) )

694

{

695

cellDmaWaitTagStatusAll(DMA_MASK(1));

696

dmaConvexVertexData (&lsMemPtr->convexVertexData[0], (btConvexHullShape*)&convexHullShape0);

697

lsMemPtr->convexVertexData[0].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[0];

698

}

699

700

701

if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )

702

{

703

cellDmaWaitTagStatusAll(DMA_MASK(1));

704

dmaConvexVertexData (&lsMemPtr->convexVertexData[1], (btConvexHullShape*)&convexHullShape1);

705

lsMemPtr->convexVertexData[1].gSpuConvexShapePtr = wuInput->m_spuCollisionShapes[1];

706

}

707

708

709

btConvexPointCloudShape cpc0,cpc1;

710

711

if ( btLikely( wuInput->m_shapeType0 == CONVEX_HULL_SHAPE_PROXYTYPE ) )

712

{

713

cellDmaWaitTagStatusAll(DMA_MASK(2));

714

lsMemPtr->convexVertexData[0].gConvexPoints = &lsMemPtr->convexVertexData[0].g_convexPointBuffer[0];

715

btConvexHullShape* ch = (btConvexHullShape*)wuInput->m_spuCollisionShapes[0];

716

const btVector3& localScaling = ch->getLocalScalingNV();

717

cpc0.setPoints(lsMemPtr->convexVertexData[0].gConvexPoints,lsMemPtr->convexVertexData[0].gNumConvexPoints,false,localScaling);

718

wuInput->m_spuCollisionShapes[0] = &cpc0;

719

}

720

721

if ( btLikely( wuInput->m_shapeType1 == CONVEX_HULL_SHAPE_PROXYTYPE ) )

722

{

723

cellDmaWaitTagStatusAll(DMA_MASK(2));

724

lsMemPtr->convexVertexData[1].gConvexPoints = &lsMemPtr->convexVertexData[1].g_convexPointBuffer[0];

725

btConvexHullShape* ch = (btConvexHullShape*)wuInput->m_spuCollisionShapes[1];

726

const btVector3& localScaling = ch->getLocalScalingNV();

727

cpc1.setPoints(lsMemPtr->convexVertexData[1].gConvexPoints,lsMemPtr->convexVertexData[1].gNumConvexPoints,false,localScaling);

728

wuInput->m_spuCollisionShapes[1] = &cpc1;

729

730

}

731

732

733

const btConvexShape* shape0Ptr = (const btConvexShape*)wuInput->m_spuCollisionShapes[0];

734

const btConvexShape* shape1Ptr = (const btConvexShape*)wuInput->m_spuCollisionShapes[1];

735

int shapeType0 = wuInput->m_shapeType0;

736

int shapeType1 = wuInput->m_shapeType1;

737

float marginA = wuInput->m_collisionMargin0;

738

float marginB = wuInput->m_collisionMargin1;

739

740

SpuClosestPointInput cpInput;

741

cpInput.m_convexVertexData[0] = &lsMemPtr->convexVertexData[0];

742

cpInput.m_convexVertexData[1] = &lsMemPtr->convexVertexData[1];

743

cpInput.m_transformA = wuInput->m_worldTransform0;

744

cpInput.m_transformB = wuInput->m_worldTransform1;

745

float sumMargin = (marginA+marginB+lsMemPtr->getContactManifoldPtr()->getContactBreakingThreshold());

746

cpInput.m_maximumDistanceSquared = sumMargin * sumMargin;

747

748

ppu_address_t manifoldAddress = (ppu_address_t)manifold;

749

750

btPersistentManifold* spuManifold=lsMemPtr->getContactManifoldPtr();

751

//spuContacts.setContactInfo(spuManifold,manifoldAddress,wuInput->m_worldTransform0,wuInput->m_worldTransform1,wuInput->m_isSwapped);

752

spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMemPtr->getColObj0()->getWorldTransform(),

753

lsMemPtr->getColObj1()->getWorldTransform(),

754

lsMemPtr->getColObj0()->getRestitution(),lsMemPtr->getColObj1()->getRestitution(),

755

lsMemPtr->getColObj0()->getFriction(),lsMemPtr->getColObj1()->getFriction(),

756

wuInput->m_isSwapped);

757

758

{

759

btGjkPairDetector gjk(shape0Ptr,shape1Ptr,shapeType0,shapeType1,marginA,marginB,&simplexSolver,penetrationSolver);//&vsSolver,penetrationSolver);

760

gjk.getClosestPoints(cpInput,spuContacts,0);//,debugDraw);

761

762

stats[gjk.m_lastUsedMethod]++;

763

degenerateStats[gjk.m_degenerateSimplex]++;

764

765

#ifdef USE_SEPDISTANCE_UTIL

766

btScalar sepDist = gjk.getCachedSeparatingDistance()+spuManifold->getContactBreakingThreshold();

767

lsMemPtr->getlocalCollisionAlgorithm()->m_sepDistance.initSeparatingDistance(gjk.getCachedSeparatingAxis(),sepDist,wuInput->m_worldTransform0,wuInput->m_worldTransform1);

768

lsMemPtr->needsDmaPutContactManifoldAlgo = true;

769

#endif //USE_SEPDISTANCE_UTIL

770

771

}

772

773

}

774

775

776

}

777

778

779

template<typename T> void DoSwap(T& a, T& b)

780

{

781

char tmp[sizeof(T)];

782

memcpy(tmp, &a, sizeof(T));

783

memcpy(&a, &b, sizeof(T));

784

memcpy(&b, tmp, sizeof(T));

785

}

786

787

SIMD_FORCE_INLINE void dmaAndSetupCollisionObjects(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem)

788

{

789

790

791

792

dmaSize = sizeof(btCollisionObject);//btTransform);

793

dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr1->m_clientObject :*/ (ppu_address_t)lsMem.getlocalCollisionAlgorithm()->getCollisionObject0();

794

lsMem.m_lsColObj0Ptr = (btCollisionObject*)cellDmaGetReadOnly(&lsMem.gColObj0Buffer, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);

795

796

dmaSize = sizeof(btCollisionObject);//btTransform);

797

dmaPpuAddress2 = /*collisionPairInput.m_isSwapped ? (ppu_address_t)lsMem.gProxyPtr0->m_clientObject :*/ (ppu_address_t)lsMem.getlocalCollisionAlgorithm()->getCollisionObject1();

798

lsMem.m_lsColObj1Ptr = (btCollisionObject*)cellDmaGetReadOnly(&lsMem.gColObj1Buffer, dmaPpuAddress2 , dmaSize, DMA_TAG(2), 0, 0);

799

800

cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));

801

802

btCollisionObject* ob0 = lsMem.getColObj0();

803

btCollisionObject* ob1 = lsMem.getColObj1();

804

805

collisionPairInput.m_worldTransform0 = ob0->getWorldTransform();

806

collisionPairInput.m_worldTransform1 = ob1->getWorldTransform();

807

}

808

809

810

811

void handleCollisionPair(SpuCollisionPairInput& collisionPairInput, CollisionTask_LocalStoreMemory& lsMem,

812

SpuContactResult &spuContacts,

813

ppu_address_t collisionShape0Ptr, void* collisionShape0Loc,

814

ppu_address_t collisionShape1Ptr, void* collisionShape1Loc, bool dmaShapes = true)

815

{

816

817

if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0)

818

&& btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1))

819

{

820

if (dmaShapes)

821

{

822

dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);

823

dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);

824

cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));

825

}

826

827

btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc;

828

btConvexInternalShape* spuConvexShape1 = (btConvexInternalShape*)collisionShape1Loc;

829

830

btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions();

831

btVector3 dim1 = spuConvexShape1->getImplicitShapeDimensions();

832

833

collisionPairInput.m_primitiveDimensions0 = dim0;

834

collisionPairInput.m_primitiveDimensions1 = dim1;

835

collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr;

836

collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr;

837

collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0;

838

collisionPairInput.m_spuCollisionShapes[1] = spuConvexShape1;

839

ProcessSpuConvexConvexCollision(&collisionPairInput,&lsMem,spuContacts);

840

}

841

else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType0) &&

842

btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType1))

843

{

844

//snPause();

845

846

dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);

847

dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);

848

cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));

849

850

// Both are compounds, do N^2 CD for now

851

///@todo: add some AABB-based pruning (probably not -> slower)

852

853

btCompoundShape* spuCompoundShape0 = (btCompoundShape*)collisionShape0Loc;

854

btCompoundShape* spuCompoundShape1 = (btCompoundShape*)collisionShape1Loc;

855

856

dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape0, 1);

857

dmaCompoundShapeInfo (&lsMem.compoundShapeData[1], spuCompoundShape1, 2);

858

cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));

859

860

861

dmaCompoundSubShapes (&lsMem.compoundShapeData[0], spuCompoundShape0, 1);

862

cellDmaWaitTagStatusAll(DMA_MASK(1));

863

dmaCompoundSubShapes (&lsMem.compoundShapeData[1], spuCompoundShape1, 1);

864

cellDmaWaitTagStatusAll(DMA_MASK(1));

865

866

int childShapeCount0 = spuCompoundShape0->getNumChildShapes();

867

btAssert(childShapeCount0< MAX_SPU_COMPOUND_SUBSHAPES);

868

int childShapeCount1 = spuCompoundShape1->getNumChildShapes();

869

btAssert(childShapeCount1< MAX_SPU_COMPOUND_SUBSHAPES);

870

871

// Start the N^2

872

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

873

{

874

btCompoundShapeChild& childShape0 = lsMem.compoundShapeData[0].gSubshapes[i];

875

btAssert(!btBroadphaseProxy::isCompound(childShape0.m_childShapeType));

876

877

for (int j = 0; j < childShapeCount1; ++j)

878

{

879

btCompoundShapeChild& childShape1 = lsMem.compoundShapeData[1].gSubshapes[j];

880

btAssert(!btBroadphaseProxy::isCompound(childShape1.m_childShapeType));

881

882

883

/* Create a new collision pair input struct using the two child shapes */

884

SpuCollisionPairInput cinput (collisionPairInput);

885

886

cinput.m_worldTransform0 = collisionPairInput.m_worldTransform0 * childShape0.m_transform;

887

cinput.m_shapeType0 = childShape0.m_childShapeType;

888

cinput.m_collisionMargin0 = childShape0.m_childMargin;

889

890

cinput.m_worldTransform1 = collisionPairInput.m_worldTransform1 * childShape1.m_transform;

891

cinput.m_shapeType1 = childShape1.m_childShapeType;

892

cinput.m_collisionMargin1 = childShape1.m_childMargin;

893

/* Recursively call handleCollisionPair () with new collision pair input */

894

895

handleCollisionPair(cinput, lsMem, spuContacts,

896

(ppu_address_t)childShape0.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i],

897

(ppu_address_t)childShape1.m_childShape, lsMem.compoundShapeData[1].gSubshapeShape[j], false);

898

}

899

}

900

}

901

else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType0) )

902

{

903

//snPause();

904

905

dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);

906

dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);

907

cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));

908

909

// object 0 compound, object 1 non-compound

910

btCompoundShape* spuCompoundShape = (btCompoundShape*)collisionShape0Loc;

911

dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape, 1);

912

cellDmaWaitTagStatusAll(DMA_MASK(1));

913

914

int childShapeCount = spuCompoundShape->getNumChildShapes();

915

btAssert(childShapeCount< MAX_SPU_COMPOUND_SUBSHAPES);

916

917

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

918

{

919

btCompoundShapeChild& childShape = lsMem.compoundShapeData[0].gSubshapes[i];

920

btAssert(!btBroadphaseProxy::isCompound(childShape.m_childShapeType));

921

// Dma the child shape

922

dmaCollisionShape (&lsMem.compoundShapeData[0].gSubshapeShape[i], (ppu_address_t)childShape.m_childShape, 1, childShape.m_childShapeType);

923

cellDmaWaitTagStatusAll(DMA_MASK(1));

924

925

SpuCollisionPairInput cinput (collisionPairInput);

926

cinput.m_worldTransform0 = collisionPairInput.m_worldTransform0 * childShape.m_transform;

927

cinput.m_shapeType0 = childShape.m_childShapeType;

928

cinput.m_collisionMargin0 = childShape.m_childMargin;

929

930

handleCollisionPair(cinput, lsMem, spuContacts,

931

(ppu_address_t)childShape.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i],

932

collisionShape1Ptr, collisionShape1Loc, false);

933

}

934

}

935

else if (btBroadphaseProxy::isCompound(collisionPairInput.m_shapeType1) )

936

{

937

//snPause();

938

939

dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);

940

dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);

941

cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));

942

// object 0 non-compound, object 1 compound

943

btCompoundShape* spuCompoundShape = (btCompoundShape*)collisionShape1Loc;

944

dmaCompoundShapeInfo (&lsMem.compoundShapeData[0], spuCompoundShape, 1);

945

cellDmaWaitTagStatusAll(DMA_MASK(1));

946

947

int childShapeCount = spuCompoundShape->getNumChildShapes();

948

btAssert(childShapeCount< MAX_SPU_COMPOUND_SUBSHAPES);

949

950

951

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

952

{

953

btCompoundShapeChild& childShape = lsMem.compoundShapeData[0].gSubshapes[i];

954

btAssert(!btBroadphaseProxy::isCompound(childShape.m_childShapeType));

955

// Dma the child shape

956

dmaCollisionShape (&lsMem.compoundShapeData[0].gSubshapeShape[i], (ppu_address_t)childShape.m_childShape, 1, childShape.m_childShapeType);

957

cellDmaWaitTagStatusAll(DMA_MASK(1));

958

959

SpuCollisionPairInput cinput (collisionPairInput);

960

cinput.m_worldTransform1 = collisionPairInput.m_worldTransform1 * childShape.m_transform;

961

cinput.m_shapeType1 = childShape.m_childShapeType;

962

cinput.m_collisionMargin1 = childShape.m_childMargin;

963

handleCollisionPair(cinput, lsMem, spuContacts,

964

collisionShape0Ptr, collisionShape0Loc,

965

(ppu_address_t)childShape.m_childShape, lsMem.compoundShapeData[0].gSubshapeShape[i], false);

966

}

967

968

}

969

else

970

{

971

//a non-convex shape is involved

972

bool handleConvexConcave = false;

973

974

//snPause();

975

976

if (btBroadphaseProxy::isConcave(collisionPairInput.m_shapeType0) &&

977

btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType1))

978

{

979

// Swap stuff

980

DoSwap(collisionShape0Ptr, collisionShape1Ptr);

981

DoSwap(collisionShape0Loc, collisionShape1Loc);

982

DoSwap(collisionPairInput.m_shapeType0, collisionPairInput.m_shapeType1);

983

DoSwap(collisionPairInput.m_worldTransform0, collisionPairInput.m_worldTransform1);

984

DoSwap(collisionPairInput.m_collisionMargin0, collisionPairInput.m_collisionMargin1);

985

986

collisionPairInput.m_isSwapped = true;

987

}

988

989

if (btBroadphaseProxy::isConvex(collisionPairInput.m_shapeType0)&&

990

btBroadphaseProxy::isConcave(collisionPairInput.m_shapeType1))

991

{

992

handleConvexConcave = true;

993

}

994

if (handleConvexConcave)

995

{

996

if (dmaShapes)

997

{

998

dmaCollisionShape (collisionShape0Loc, collisionShape0Ptr, 1, collisionPairInput.m_shapeType0);

999

dmaCollisionShape (collisionShape1Loc, collisionShape1Ptr, 2, collisionPairInput.m_shapeType1);

1000

cellDmaWaitTagStatusAll(DMA_MASK(1) | DMA_MASK(2));

1001

}

1002

1003

if (collisionPairInput.m_shapeType1 == STATIC_PLANE_PROXYTYPE)

1004

{

1005

btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc;

1006

btStaticPlaneShape* planeShape= (btStaticPlaneShape*)collisionShape1Loc;

1007

1008

btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions();

1009

collisionPairInput.m_primitiveDimensions0 = dim0;

1010

collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr;

1011

collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr;

1012

collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0;

1013

collisionPairInput.m_spuCollisionShapes[1] = planeShape;

1014

1015

ProcessConvexPlaneSpuCollision(&collisionPairInput,&lsMem,spuContacts);

1016

} else

1017

{

1018

btConvexInternalShape* spuConvexShape0 = (btConvexInternalShape*)collisionShape0Loc;

1019

btBvhTriangleMeshShape* trimeshShape = (btBvhTriangleMeshShape*)collisionShape1Loc;

1020

1021

btVector3 dim0 = spuConvexShape0->getImplicitShapeDimensions();

1022

collisionPairInput.m_primitiveDimensions0 = dim0;

1023

collisionPairInput.m_collisionShapes[0] = collisionShape0Ptr;

1024

collisionPairInput.m_collisionShapes[1] = collisionShape1Ptr;

1025

collisionPairInput.m_spuCollisionShapes[0] = spuConvexShape0;

1026

collisionPairInput.m_spuCollisionShapes[1] = trimeshShape;

1027

1028

ProcessConvexConcaveSpuCollision(&collisionPairInput,&lsMem,spuContacts);

1029

}

1030

}

1031

1032

}

1033

1034

spuContacts.flush();

1035

1036

}

1037

1038

1039

void processCollisionTask(void* userPtr, void* lsMemPtr)

1040

{

1041

1042

SpuGatherAndProcessPairsTaskDesc* taskDescPtr = (SpuGatherAndProcessPairsTaskDesc*)userPtr;

1043

SpuGatherAndProcessPairsTaskDesc& taskDesc = *taskDescPtr;

1044

CollisionTask_LocalStoreMemory* colMemPtr = (CollisionTask_LocalStoreMemory*)lsMemPtr;

1045

CollisionTask_LocalStoreMemory& lsMem = *(colMemPtr);

1046

1047

gUseEpa = taskDesc.m_useEpa;

1048

1049

// spu_printf("taskDescPtr=%llx\n",taskDescPtr);

1050

1051

SpuContactResult spuContacts;

1052

1053

////////////////////

1054

1055

ppu_address_t dmaInPtr = taskDesc.m_inPairPtr;

1056

unsigned int numPages = taskDesc.numPages;

1057

unsigned int numOnLastPage = taskDesc.numOnLastPage;

1058

1059

// prefetch first set of inputs and wait

1060

lsMem.g_workUnitTaskBuffers.init();

1061

1062

unsigned int nextNumOnPage = (numPages > 1)? MIDPHASE_NUM_WORKUNITS_PER_PAGE : numOnLastPage;

1063

lsMem.g_workUnitTaskBuffers.backBufferDmaGet(dmaInPtr, nextNumOnPage*sizeof(SpuGatherAndProcessWorkUnitInput), DMA_TAG(3));

1064

dmaInPtr += MIDPHASE_WORKUNIT_PAGE_SIZE;

1065

1066

1067

1068

1069

1070

SpuGatherAndProcessWorkUnitInput* wuInputs;

1071

1072

1073

1074

1075

int numPairs;

1076

1077

SpuCollisionPairInput collisionPairInput;

1078

1079

for (unsigned int i = 0; btLikely(i < numPages); i++)

1080

{

1081

1082

// wait for back buffer dma and swap buffers

1083

inputPtr = lsMem.g_workUnitTaskBuffers.swapBuffers();

1084

1085

// number on current page is number prefetched last iteration

1086

numOnPage = nextNumOnPage;

1087

1088

1089

// prefetch next set of inputs

1090

#if MIDPHASE_NUM_WORKUNIT_PAGES > 2

1091

if ( btLikely( i < numPages-1 ) )

1092

#else

1093

if ( btUnlikely( i < numPages-1 ) )

1094

#endif

1095

{

1096

nextNumOnPage = (i == numPages-2)? numOnLastPage : MIDPHASE_NUM_WORKUNITS_PER_PAGE;

1097

lsMem.g_workUnitTaskBuffers.backBufferDmaGet(dmaInPtr, nextNumOnPage*sizeof(SpuGatherAndProcessWorkUnitInput), DMA_TAG(3));

1098

dmaInPtr += MIDPHASE_WORKUNIT_PAGE_SIZE;

1099

}

1100

1101

wuInputs = reinterpret_cast<SpuGatherAndProcessWorkUnitInput *>(inputPtr);

1102

1103

1104

for (j = 0; btLikely( j < numOnPage ); j++)

1105

{

1106

#ifdef DEBUG_SPU_COLLISION_DETECTION

1107

// printMidphaseInput(&wuInputs[j]);

1108

#endif //DEBUG_SPU_COLLISION_DETECTION

1109

1110

1111

numPairs = wuInputs[j].m_endIndex - wuInputs[j].m_startIndex;

1112

1113

if ( btLikely( numPairs ) )

1114

{

1115

dmaSize = numPairs*sizeof(btBroadphasePair);

1116

dmaPpuAddress = wuInputs[j].m_pairArrayPtr+wuInputs[j].m_startIndex * sizeof(btBroadphasePair);

1117

lsMem.m_pairsPointer = (btBroadphasePair*)cellDmaGetReadOnly(&lsMem.gBroadphasePairsBuffer, dmaPpuAddress , dmaSize, DMA_TAG(1), 0, 0);

1118

cellDmaWaitTagStatusAll(DMA_MASK(1));

1119

1120

1121

for (p=0;p<numPairs;p++)

1122

{

1123

1124

//for each broadphase pair, do something

1125

1126

btBroadphasePair& pair = lsMem.getBroadphasePairPtr()[p];

1127

#ifdef DEBUG_SPU_COLLISION_DETECTION

1128

spu_printf("pair->m_userInfo = %d\n",pair.m_userInfo);

1129

spu_printf("pair->m_algorithm = %d\n",pair.m_algorithm);

1130

spu_printf("pair->m_pProxy0 = %d\n",pair.m_pProxy0);

1131

spu_printf("pair->m_pProxy1 = %d\n",pair.m_pProxy1);

1132

#endif //DEBUG_SPU_COLLISION_DETECTION

1133

1134

if (pair.m_internalTmpValue == 2 && pair.m_algorithm && pair.m_pProxy0 && pair.m_pProxy1)

1135

{

1136

dmaSize = sizeof(SpuContactManifoldCollisionAlgorithm);

1137

dmaPpuAddress2 = (ppu_address_t)pair.m_algorithm;

1138

lsMem.m_lsCollisionAlgorithmPtr = (SpuContactManifoldCollisionAlgorithm*)cellDmaGetReadOnly(&lsMem.gSpuContactManifoldAlgoBuffer, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);

1139

1140

cellDmaWaitTagStatusAll(DMA_MASK(1));

1141

1142

lsMem.needsDmaPutContactManifoldAlgo = false;

1143

1144

collisionPairInput.m_persistentManifoldPtr = (ppu_address_t) lsMem.getlocalCollisionAlgorithm()->getContactManifoldPtr();

1145

collisionPairInput.m_isSwapped = false;

1146

1147

if (1)

1148

{

1149

1150

///can wait on the combined DMA_MASK, or dma on the same tag

1151

1152

1153

#ifdef DEBUG_SPU_COLLISION_DETECTION

1154

// spu_printf("SPU collisionPairInput->m_shapeType0 = %d\n",collisionPairInput->m_shapeType0);

1155

// spu_printf("SPU collisionPairInput->m_shapeType1 = %d\n",collisionPairInput->m_shapeType1);

1156

#endif //DEBUG_SPU_COLLISION_DETECTION

1157

1158

1159

dmaSize = sizeof(btPersistentManifold);

1160

1161

dmaPpuAddress2 = collisionPairInput.m_persistentManifoldPtr;

1162

lsMem.m_lsManifoldPtr = (btPersistentManifold*)cellDmaGetReadOnly(&lsMem.gPersistentManifoldBuffer, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);

1163

1164

collisionPairInput.m_shapeType0 = lsMem.getlocalCollisionAlgorithm()->getShapeType0();

1165

collisionPairInput.m_shapeType1 = lsMem.getlocalCollisionAlgorithm()->getShapeType1();

1166

collisionPairInput.m_collisionMargin0 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin0();

1167

collisionPairInput.m_collisionMargin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1();

1168

1169

1170

1171

//??cellDmaWaitTagStatusAll(DMA_MASK(1));

1172

1173

1174

if (1)

1175

{

1176

//snPause();

1177

1178

// Get the collision objects

1179

dmaAndSetupCollisionObjects(collisionPairInput, lsMem);

1180

1181

if (lsMem.getColObj0()->isActive() || lsMem.getColObj1()->isActive())

1182

{

1183

1184

lsMem.needsDmaPutContactManifoldAlgo = true;

1185

#ifdef USE_SEPDISTANCE_UTIL

1186

lsMem.getlocalCollisionAlgorithm()->m_sepDistance.updateSeparatingDistance(collisionPairInput.m_worldTransform0,collisionPairInput.m_worldTransform1);

1187

#endif //USE_SEPDISTANCE_UTIL

1188

1189

#define USE_DEDICATED_BOX_BOX 1

1190

#ifdef USE_DEDICATED_BOX_BOX

1191

bool boxbox = ((lsMem.getlocalCollisionAlgorithm()->getShapeType0()==BOX_SHAPE_PROXYTYPE)&&

1192

(lsMem.getlocalCollisionAlgorithm()->getShapeType1()==BOX_SHAPE_PROXYTYPE));

1193

if (boxbox)

1194

{

1195

//spu_printf("boxbox dist = %f\n",distance);

1196

btPersistentManifold* spuManifold=lsMem.getContactManifoldPtr();

1197

btPersistentManifold* manifold = (btPersistentManifold*)collisionPairInput.m_persistentManifoldPtr;

1198

ppu_address_t manifoldAddress = (ppu_address_t)manifold;

1199

1200

spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMem.getColObj0()->getWorldTransform(),

1201

lsMem.getColObj1()->getWorldTransform(),

1202

lsMem.getColObj0()->getRestitution(),lsMem.getColObj1()->getRestitution(),

1203

lsMem.getColObj0()->getFriction(),lsMem.getColObj1()->getFriction(),

1204

collisionPairInput.m_isSwapped);

1205

1206

1207

//float distance=0.f;

1208

btVector3 normalInB;

1209

1210

1211

if (//!gUseEpa &&

1212

#ifdef USE_SEPDISTANCE_UTIL

1213

lsMem.getlocalCollisionAlgorithm()->m_sepDistance.getConservativeSeparatingDistance()<=0.f

1214

#else

1215

1216

#endif

1217

)

1218

{

1219

//#define USE_PE_BOX_BOX 1

1220

#ifdef USE_PE_BOX_BOX

1221

{

1222

1223

//getCollisionMargin0

1224

btScalar margin0 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin0();

1225

btScalar margin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1();

1226

btVector3 shapeDim0 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions0()+btVector3(margin0,margin0,margin0);

1227

btVector3 shapeDim1 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions1()+btVector3(margin1,margin1,margin1);

1228

1229

//Box boxA(shapeDim0.getX(),shapeDim0.getY(),shapeDim0.getZ());

1230

vmVector3 vmPos0 = getVmVector3(collisionPairInput.m_worldTransform0.getOrigin());

1231

vmVector3 vmPos1 = getVmVector3(collisionPairInput.m_worldTransform1.getOrigin());

1232

vmMatrix3 vmMatrix0 = getVmMatrix3(collisionPairInput.m_worldTransform0.getBasis());

1233

vmMatrix3 vmMatrix1 = getVmMatrix3(collisionPairInput.m_worldTransform1.getBasis());

1234

1235

vmTransform3 transformA(vmMatrix0,vmPos0);

1236

Box boxB(shapeDim1.getX(),shapeDim1.getY(),shapeDim1.getZ());

1237

vmTransform3 transformB(vmMatrix1,vmPos1);

1238

BoxPoint resultClosestBoxPointA;

1239

BoxPoint resultClosestBoxPointB;

1240

vmVector3 resultNormal;

1241

1242

1243

#ifdef USE_SEPDISTANCE_UTIL

1244

float distanceThreshold = FLT_MAX

1245

#else

1246

//float distanceThreshold = 0.f;

1247

#endif

1248

1249

1250

vmVector3 n;

1251

Box boxA;

1252

vmVector3 hA(shapeDim0.getX(),shapeDim0.getY(),shapeDim0.getZ());

1253

vmVector3 hB(shapeDim1.getX(),shapeDim1.getY(),shapeDim1.getZ());

1254

boxA.mHalf= hA;

1255

vmTransform3 trA;

1256

trA.setTranslation(getVmVector3(collisionPairInput.m_worldTransform0.getOrigin()));

1257

trA.setUpper3x3(getVmMatrix3(collisionPairInput.m_worldTransform0.getBasis()));

1258

Box boxB;

1259

boxB.mHalf = hB;

1260

vmTransform3 trB;

1261

trB.setTranslation(getVmVector3(collisionPairInput.m_worldTransform1.getOrigin()));

1262

trB.setUpper3x3(getVmMatrix3(collisionPairInput.m_worldTransform1.getBasis()));

1263

1264

float distanceThreshold = spuManifold->getContactBreakingThreshold();//0.001f;

1265

1266

1267

BoxPoint ptA,ptB;

1268

float dist = boxBoxDistance(n, ptA, ptB,

1269

boxA, trA, boxB, trB,

1270

distanceThreshold );

1271

1272

1273

// float distance = boxBoxDistance(resultNormal,resultClosestBoxPointA,resultClosestBoxPointB, boxA, transformA, boxB,transformB,distanceThreshold);

1274

1275

normalInB = -getBtVector3(n);//resultNormal);

1276

1277

//if(dist < distanceThreshold)//spuManifold->getContactBreakingThreshold())

1278

if(dist < spuManifold->getContactBreakingThreshold())

1279

{

1280

btVector3 pointOnB = collisionPairInput.m_worldTransform1(getBtVector3(ptB.localPoint));

1281

1282

spuContacts.addContactPoint(

1283

normalInB,

1284

pointOnB,

1285

dist);

1286

}

1287

}

1288

#else

1289

{

1290

1291

btScalar margin0 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin0();

1292

btScalar margin1 = lsMem.getlocalCollisionAlgorithm()->getCollisionMargin1();

1293

btVector3 shapeDim0 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions0()+btVector3(margin0,margin0,margin0);

1294

btVector3 shapeDim1 = lsMem.getlocalCollisionAlgorithm()->getShapeDimensions1()+btVector3(margin1,margin1,margin1);

1295

1296

1297

btBoxShape box0(shapeDim0);

1298

btBoxShape box1(shapeDim1);

1299

1300

struct SpuBridgeContactCollector : public btDiscreteCollisionDetectorInterface::Result

1301

{

1302

SpuContactResult& m_spuContacts;

1303

1304

virtual void setShapeIdentifiersA(int partId0,int index0)

1305

{

1306

m_spuContacts.setShapeIdentifiersA(partId0,index0);

1307

}

1308

virtual void setShapeIdentifiersB(int partId1,int index1)

1309

{

1310

m_spuContacts.setShapeIdentifiersB(partId1,index1);

1311

}

1312

virtual void addContactPoint(const btVector3& normalOnBInWorld,const btVector3& pointInWorld,btScalar depth)

1313

{

1314

m_spuContacts.addContactPoint(normalOnBInWorld,pointInWorld,depth);

1315

}

1316

1317

SpuBridgeContactCollector(SpuContactResult& spuContacts)

1318

:m_spuContacts(spuContacts)

1319

{

1320

1321

}

1322

};

1323

1324

SpuBridgeContactCollector bridgeOutput(spuContacts);

1325

1326

btDiscreteCollisionDetectorInterface::ClosestPointInput input;

1327

input.m_maximumDistanceSquared = BT_LARGE_FLOAT;

1328

input.m_transformA = collisionPairInput.m_worldTransform0;

1329

input.m_transformB = collisionPairInput.m_worldTransform1;

1330

1331

btBoxBoxDetector detector(&box0,&box1);

1332

1333

detector.getClosestPoints(input,bridgeOutput,0);

1334

1335

}

1336

#endif //USE_PE_BOX_BOX

1337

1338

lsMem.needsDmaPutContactManifoldAlgo = true;

1339

#ifdef USE_SEPDISTANCE_UTIL

1340

btScalar sepDist2 = distance+spuManifold->getContactBreakingThreshold();

1341

lsMem.getlocalCollisionAlgorithm()->m_sepDistance.initSeparatingDistance(normalInB,sepDist2,collisionPairInput.m_worldTransform0,collisionPairInput.m_worldTransform1);

1342

#endif //USE_SEPDISTANCE_UTIL

1343

gProcessedCol++;

1344

} else

1345

{

1346

gSkippedCol++;

1347

}

1348

1349

spuContacts.flush();

1350

1351

1352

} else

1353

#endif //USE_DEDICATED_BOX_BOX

1354

{

1355

if (

1356

#ifdef USE_SEPDISTANCE_UTIL

1357

lsMem.getlocalCollisionAlgorithm()->m_sepDistance.getConservativeSeparatingDistance()<=0.f

1358

#else

1359

1360

#endif //USE_SEPDISTANCE_UTIL

1361

)

1362

{

1363

handleCollisionPair(collisionPairInput, lsMem, spuContacts,

1364

(ppu_address_t)lsMem.getColObj0()->getRootCollisionShape(), &lsMem.gCollisionShapes[0].collisionShape,

1365

(ppu_address_t)lsMem.getColObj1()->getRootCollisionShape(), &lsMem.gCollisionShapes[1].collisionShape);

1366

} else

1367

{

1368

//spu_printf("boxbox dist = %f\n",distance);

1369

btPersistentManifold* spuManifold=lsMem.getContactManifoldPtr();

1370

btPersistentManifold* manifold = (btPersistentManifold*)collisionPairInput.m_persistentManifoldPtr;

1371

ppu_address_t manifoldAddress = (ppu_address_t)manifold;

1372

1373

spuContacts.setContactInfo(spuManifold,manifoldAddress,lsMem.getColObj0()->getWorldTransform(),

1374

lsMem.getColObj1()->getWorldTransform(),

1375

lsMem.getColObj0()->getRestitution(),lsMem.getColObj1()->getRestitution(),

1376

lsMem.getColObj0()->getFriction(),lsMem.getColObj1()->getFriction(),

1377

collisionPairInput.m_isSwapped);

1378

1379

spuContacts.flush();

1380

}

1381

}

1382

1383

}

1384

1385

}

1386

}

1387

1388

#ifdef USE_SEPDISTANCE_UTIL

1389

#if defined (__SPU__) || defined (USE_LIBSPE2)

1390

if (lsMem.needsDmaPutContactManifoldAlgo)

1391

{

1392

dmaSize = sizeof(SpuContactManifoldCollisionAlgorithm);

1393

dmaPpuAddress2 = (ppu_address_t)pair.m_algorithm;

1394

cellDmaLargePut(&lsMem.gSpuContactManifoldAlgoBuffer, dmaPpuAddress2 , dmaSize, DMA_TAG(1), 0, 0);

1395

cellDmaWaitTagStatusAll(DMA_MASK(1));

1396

}

1397

#endif

1398

#endif //#ifdef USE_SEPDISTANCE_UTIL

1399

1400

}

1401

}

1402

}

1403

} //end for (j = 0; j < numOnPage; j++)

1404

1405

}// for

1406

1407

1408

1409

return;

1410

}

1411

1412

Older »