~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 "PfxContactBoxBox.h"

#include <math.h>

#include "../PlatformDefinitions.h"

#include "boxBoxDistance.h"

static inline float sqr( float a )

{

return (a * a);

}

enum BoxSepAxisType

{

A_AXIS, B_AXIS, CROSS_AXIS

};

//-------------------------------------------------------------------------------------------------

// voronoiTol: bevels Voronoi planes slightly which helps when features are parallel.

//-------------------------------------------------------------------------------------------------

static const float voronoiTol = -1.0e-5f;

//-------------------------------------------------------------------------------------------------

// separating axis tests: gaps along each axis are computed, and the axis with the maximum

// gap is stored. cross product axes are normalized.

//-------------------------------------------------------------------------------------------------

#define AaxisTest( dim, letter, first ) \

{ \

if ( first ) \

{ \

maxGap = gap = gapsA.get##letter(); \

if ( gap > distanceThreshold ) return gap; \

axisType = A_AXIS; \

faceDimA = dim; \

axisA = identity.getCol##dim(); \

} \

else \

{ \

gap = gapsA.get##letter(); \

if ( gap > distanceThreshold ) return gap; \

else if ( gap > maxGap ) \

{ \

maxGap = gap; \

axisType = A_AXIS; \

faceDimA = dim; \

axisA = identity.getCol##dim(); \

} \

}

#define BaxisTest( dim, letter ) \

{ \

gap = gapsB.get##letter(); \

if ( gap > distanceThreshold ) return gap; \

else if ( gap > maxGap ) \

{ \

maxGap = gap; \

axisType = B_AXIS; \

faceDimB = dim; \

axisB = identity.getCol##dim(); \

} \

}

#define CrossAxisTest( dima, dimb, letterb ) \

{ \

const float lsqr_tolerance = 1.0e-30f; \

float lsqr; \

lsqr = lsqrs.getCol##dima().get##letterb(); \

if ( lsqr > lsqr_tolerance ) \

{ \

float l_recip = 1.0f / sqrtf( lsqr ); \

gap = float(gapsAxB.getCol##dima().get##letterb()) * l_recip; \

if ( gap > distanceThreshold ) \

{ \

return gap; \

} \

100

if ( gap > maxGap ) \

101

{ \

102

maxGap = gap; \

103

axisType = CROSS_AXIS; \

104

edgeDimA = dima; \

105

edgeDimB = dimb; \

106

axisA = cross(identity.getCol##dima(),matrixAB.getCol##dimb()) * l_recip; \

107

} \

108

} \

109

}

110

111

//-------------------------------------------------------------------------------------------------

112

// tests whether a vertex of box B and a face of box A are the closest features

113

//-------------------------------------------------------------------------------------------------

114

115

inline

116

float

117

VertexBFaceATest(

118

bool & inVoronoi,

119

float & t0,

120

float & t1,

121

const vmVector3 & hA,

122

PE_REF(vmVector3) faceOffsetAB,

123

PE_REF(vmVector3) faceOffsetBA,

124

const vmMatrix3 & matrixAB,

125

const vmMatrix3 & matrixBA,

126

PE_REF(vmVector3) signsB,

127

PE_REF(vmVector3) scalesB )

128

{

129

// compute a corner of box B in A's coordinate system

130

131

vmVector3 corner =

132

vmVector3( faceOffsetAB + matrixAB.getCol0() * scalesB.getX() + matrixAB.getCol1() * scalesB.getY() );

133

134

// compute the parameters of the point on A, closest to this corner

135

136

t0 = corner[0];

137

t1 = corner[1];

138

139

if ( t0 > hA[0] )

140

t0 = hA[0];

141

else if ( t0 < -hA[0] )

142

t0 = -hA[0];

143

if ( t1 > hA[1] )

144

t1 = hA[1];

145

else if ( t1 < -hA[1] )

146

t1 = -hA[1];

147

148

// do the Voronoi test: already know the point on B is in the Voronoi region of the

149

// point on A, check the reverse.

150

151

vmVector3 facePointB =

152

vmVector3( mulPerElem( faceOffsetBA + matrixBA.getCol0() * t0 + matrixBA.getCol1() * t1 - scalesB, signsB ) );

153

154

inVoronoi = ( ( facePointB[0] >= voronoiTol * facePointB[2] ) &&

155

( facePointB[1] >= voronoiTol * facePointB[0] ) &&

156

( facePointB[2] >= voronoiTol * facePointB[1] ) );

157

158

return (sqr( corner[0] - t0 ) + sqr( corner[1] - t1 ) + sqr( corner[2] ));

159

}

160

161

#define VertexBFaceA_SetNewMin() \

162

{ \

163

minDistSqr = distSqr; \

164

localPointA.setX(t0); \

165

localPointA.setY(t1); \

166

localPointB.setX( scalesB.getX() ); \

167

localPointB.setY( scalesB.getY() ); \

168

featureA = F; \

169

featureB = V; \

170

}

171

172

void

173

VertexBFaceATests(

174

bool & done,

175

float & minDistSqr,

176

vmPoint3 & localPointA,

177

vmPoint3 & localPointB,

178

FeatureType & featureA,

179

FeatureType & featureB,

180

const vmVector3 & hA,

181

PE_REF(vmVector3) faceOffsetAB,

182

PE_REF(vmVector3) faceOffsetBA,

183

const vmMatrix3 & matrixAB,

184

const vmMatrix3 & matrixBA,

185

PE_REF(vmVector3) signsB,

186

PE_REF(vmVector3) scalesB,

187

bool first )

188

{

189

190

float t0, t1;

191

float distSqr;

192

193

distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,

194

matrixAB, matrixBA, signsB, scalesB );

195

196

if ( first ) {

197

VertexBFaceA_SetNewMin();

198

} else {

199

if ( distSqr < minDistSqr ) {

200

VertexBFaceA_SetNewMin();

201

}

202

}

203

204

if ( done )

205

return;

206

207

signsB.setX( -signsB.getX() );

208

scalesB.setX( -scalesB.getX() );

209

210

distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,

211

matrixAB, matrixBA, signsB, scalesB );

212

213

if ( distSqr < minDistSqr ) {

214

VertexBFaceA_SetNewMin();

215

}

216

217

if ( done )

218

return;

219

220

signsB.setY( -signsB.getY() );

221

scalesB.setY( -scalesB.getY() );

222

223

distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,

224

matrixAB, matrixBA, signsB, scalesB );

225

226

if ( distSqr < minDistSqr ) {

227

VertexBFaceA_SetNewMin();

228

}

229

230

if ( done )

231

return;

232

233

signsB.setX( -signsB.getX() );

234

scalesB.setX( -scalesB.getX() );

235

236

distSqr = VertexBFaceATest( done, t0, t1, hA, faceOffsetAB, faceOffsetBA,

237

matrixAB, matrixBA, signsB, scalesB );

238

239

if ( distSqr < minDistSqr ) {

240

VertexBFaceA_SetNewMin();

241

}

242

}

243

244

//-------------------------------------------------------------------------------------------------

245

// VertexAFaceBTest: tests whether a vertex of box A and a face of box B are the closest features

246

//-------------------------------------------------------------------------------------------------

247

248

inline

249

float

250

VertexAFaceBTest(

251

bool & inVoronoi,

252

float & t0,

253

float & t1,

254

const vmVector3 & hB,

255

PE_REF(vmVector3) faceOffsetAB,

256

PE_REF(vmVector3) faceOffsetBA,

257

const vmMatrix3 & matrixAB,

258

const vmMatrix3 & matrixBA,

259

PE_REF(vmVector3) signsA,

260

PE_REF(vmVector3) scalesA )

261

{

262

vmVector3 corner =

263

vmVector3( faceOffsetBA + matrixBA.getCol0() * scalesA.getX() + matrixBA.getCol1() * scalesA.getY() );

264

265

t0 = corner[0];

266

t1 = corner[1];

267

268

if ( t0 > hB[0] )

269

t0 = hB[0];

270

else if ( t0 < -hB[0] )

271

t0 = -hB[0];

272

if ( t1 > hB[1] )

273

t1 = hB[1];

274

else if ( t1 < -hB[1] )

275

t1 = -hB[1];

276

277

vmVector3 facePointA =

278

vmVector3( mulPerElem( faceOffsetAB + matrixAB.getCol0() * t0 + matrixAB.getCol1() * t1 - scalesA, signsA ) );

279

280

inVoronoi = ( ( facePointA[0] >= voronoiTol * facePointA[2] ) &&

281

( facePointA[1] >= voronoiTol * facePointA[0] ) &&

282

( facePointA[2] >= voronoiTol * facePointA[1] ) );

283

284

return (sqr( corner[0] - t0 ) + sqr( corner[1] - t1 ) + sqr( corner[2] ));

285

}

286

287

#define VertexAFaceB_SetNewMin() \

288

{ \

289

minDistSqr = distSqr; \

290

localPointB.setX(t0); \

291

localPointB.setY(t1); \

292

localPointA.setX( scalesA.getX() ); \

293

localPointA.setY( scalesA.getY() ); \

294

featureA = V; \

295

featureB = F; \

296

}

297

298

void

299

VertexAFaceBTests(

300

bool & done,

301

float & minDistSqr,

302

vmPoint3 & localPointA,

303

vmPoint3 & localPointB,

304

FeatureType & featureA,

305

FeatureType & featureB,

306

const vmVector3 & hB,

307

PE_REF(vmVector3) faceOffsetAB,

308

PE_REF(vmVector3) faceOffsetBA,

309

const vmMatrix3 & matrixAB,

310

const vmMatrix3 & matrixBA,

311

PE_REF(vmVector3) signsA,

312

PE_REF(vmVector3) scalesA,

313

bool first )

314

{

315

float t0, t1;

316

float distSqr;

317

318

distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,

319

matrixAB, matrixBA, signsA, scalesA );

320

321

if ( first ) {

322

VertexAFaceB_SetNewMin();

323

} else {

324

if ( distSqr < minDistSqr ) {

325

VertexAFaceB_SetNewMin();

326

}

327

}

328

329

if ( done )

330

return;

331

332

signsA.setX( -signsA.getX() );

333

scalesA.setX( -scalesA.getX() );

334

335

distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,

336

matrixAB, matrixBA, signsA, scalesA );

337

338

if ( distSqr < minDistSqr ) {

339

VertexAFaceB_SetNewMin();

340

}

341

342

if ( done )

343

return;

344

345

signsA.setY( -signsA.getY() );

346

scalesA.setY( -scalesA.getY() );

347

348

distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,

349

matrixAB, matrixBA, signsA, scalesA );

350

351

if ( distSqr < minDistSqr ) {

352

VertexAFaceB_SetNewMin();

353

}

354

355

if ( done )

356

return;

357

358

signsA.setX( -signsA.getX() );

359

scalesA.setX( -scalesA.getX() );

360

361

distSqr = VertexAFaceBTest( done, t0, t1, hB, faceOffsetAB, faceOffsetBA,

362

matrixAB, matrixBA, signsA, scalesA );

363

364

if ( distSqr < minDistSqr ) {

365

VertexAFaceB_SetNewMin();

366

}

367

}

368

369

//-------------------------------------------------------------------------------------------------

370

// CustomEdgeEdgeTest:

371

372

// tests whether a pair of edges are the closest features

373

374

// note on the shorthand:

375

// 'a' & 'b' refer to the edges.

376

// 'c' is the dimension of the axis that points from the face center to the edge Center

377

// 'd' is the dimension of the edge Direction

378

// the dimension of the face normal is 2

379

//-------------------------------------------------------------------------------------------------

380

381

#define CustomEdgeEdgeTest( ac, ac_letter, ad, ad_letter, bc, bc_letter, bd, bd_letter ) \

382

{ \

383

vmVector3 edgeOffsetAB; \

384

vmVector3 edgeOffsetBA; \

385

386

edgeOffsetAB = faceOffsetAB + matrixAB.getCol##bc() * scalesB.get##bc_letter(); \

387

edgeOffsetAB.set##ac_letter( edgeOffsetAB.get##ac_letter() - scalesA.get##ac_letter() ); \

388

389

edgeOffsetBA = faceOffsetBA + matrixBA.getCol##ac() * scalesA.get##ac_letter(); \

390

edgeOffsetBA.set##bc_letter( edgeOffsetBA.get##bc_letter() - scalesB.get##bc_letter() ); \

391

392

float dirDot = matrixAB.getCol##bd().get##ad_letter(); \

393

float denom = 1.0f - dirDot*dirDot; \

394

float edgeOffsetAB_ad = edgeOffsetAB.get##ad_letter(); \

395

float edgeOffsetBA_bd = edgeOffsetBA.get##bd_letter(); \

396

397

if ( denom == 0.0f ) \

398

{ \

399

tA = 0.0f; \

400

} \

401

else \

402

{ \

403

tA = ( edgeOffsetAB_ad + edgeOffsetBA_bd * dirDot ) / denom; \

404

} \

405

406

if ( tA < -hA[ad] ) tA = -hA[ad]; \

407

else if ( tA > hA[ad] ) tA = hA[ad]; \

408

409

tB = tA * dirDot + edgeOffsetBA_bd; \

410

411

if ( tB < -hB[bd] ) \

412

{ \

413

tB = -hB[bd]; \

414

tA = tB * dirDot + edgeOffsetAB_ad; \

415

416

if ( tA < -hA[ad] ) tA = -hA[ad]; \

417

else if ( tA > hA[ad] ) tA = hA[ad]; \

418

} \

419

else if ( tB > hB[bd] ) \

420

{ \

421

tB = hB[bd]; \

422

tA = tB * dirDot + edgeOffsetAB_ad; \

423

424

if ( tA < -hA[ad] ) tA = -hA[ad]; \

425

else if ( tA > hA[ad] ) tA = hA[ad]; \

426

} \

427

428

vmVector3 edgeOffAB = vmVector3( mulPerElem( edgeOffsetAB + matrixAB.getCol##bd() * tB, signsA ) );\

429

vmVector3 edgeOffBA = vmVector3( mulPerElem( edgeOffsetBA + matrixBA.getCol##ad() * tA, signsB ) );\

430

431

inVoronoi = ( edgeOffAB[ac] >= voronoiTol * edgeOffAB[2] ) && \

432

( edgeOffAB[2] >= voronoiTol * edgeOffAB[ac] ) && \

433

( edgeOffBA[bc] >= voronoiTol * edgeOffBA[2] ) && \

434

( edgeOffBA[2] >= voronoiTol * edgeOffBA[bc] ); \

435

436

edgeOffAB[ad] -= tA; \

437

edgeOffBA[bd] -= tB; \

438

439

return dot(edgeOffAB,edgeOffAB); \

440

}

441

442

float

443

CustomEdgeEdgeTest_0101(

444

bool & inVoronoi,

445

float & tA,

446

float & tB,

447

const vmVector3 & hA,

448

const vmVector3 & hB,

449

PE_REF(vmVector3) faceOffsetAB,

450

PE_REF(vmVector3) faceOffsetBA,

451

const vmMatrix3 & matrixAB,

452

const vmMatrix3 & matrixBA,

453

PE_REF(vmVector3) signsA,

454

PE_REF(vmVector3) signsB,

455

PE_REF(vmVector3) scalesA,

456

PE_REF(vmVector3) scalesB )

457

{

458

CustomEdgeEdgeTest( 0, X, 1, Y, 0, X, 1, Y );

459

}

460

461

float

462

CustomEdgeEdgeTest_0110(

463

bool & inVoronoi,

464

float & tA,

465

float & tB,

466

const vmVector3 & hA,

467

const vmVector3 & hB,

468

PE_REF(vmVector3) faceOffsetAB,

469

PE_REF(vmVector3) faceOffsetBA,

470

const vmMatrix3 & matrixAB,

471

const vmMatrix3 & matrixBA,

472

PE_REF(vmVector3) signsA,

473

PE_REF(vmVector3) signsB,

474

PE_REF(vmVector3) scalesA,

475

PE_REF(vmVector3) scalesB )

476

{

477

CustomEdgeEdgeTest( 0, X, 1, Y, 1, Y, 0, X );

478

}

479

480

float

481

CustomEdgeEdgeTest_1001(

482

bool & inVoronoi,

483

float & tA,

484

float & tB,

485

const vmVector3 & hA,

486

const vmVector3 & hB,

487

PE_REF(vmVector3) faceOffsetAB,

488

PE_REF(vmVector3) faceOffsetBA,

489

const vmMatrix3 & matrixAB,

490

const vmMatrix3 & matrixBA,

491

PE_REF(vmVector3) signsA,

492

PE_REF(vmVector3) signsB,

493

PE_REF(vmVector3) scalesA,

494

PE_REF(vmVector3) scalesB )

495

{

496

CustomEdgeEdgeTest( 1, Y, 0, X, 0, X, 1, Y );

497

}

498

499

float

500

CustomEdgeEdgeTest_1010(

501

bool & inVoronoi,

502

float & tA,

503

float & tB,

504

const vmVector3 & hA,

505

const vmVector3 & hB,

506

PE_REF(vmVector3) faceOffsetAB,

507

PE_REF(vmVector3) faceOffsetBA,

508

const vmMatrix3 & matrixAB,

509

const vmMatrix3 & matrixBA,

510

PE_REF(vmVector3) signsA,

511

PE_REF(vmVector3) signsB,

512

PE_REF(vmVector3) scalesA,

513

PE_REF(vmVector3) scalesB )

514

{

515

CustomEdgeEdgeTest( 1, Y, 0, X, 1, Y, 0, X );

516

}

517

518

#define EdgeEdge_SetNewMin( ac_letter, ad_letter, bc_letter, bd_letter ) \

519

{ \

520

minDistSqr = distSqr; \

521

localPointA.set##ac_letter(scalesA.get##ac_letter()); \

522

localPointA.set##ad_letter(tA); \

523

localPointB.set##bc_letter(scalesB.get##bc_letter()); \

524

localPointB.set##bd_letter(tB); \

525

otherFaceDimA = testOtherFaceDimA; \

526

otherFaceDimB = testOtherFaceDimB; \

527

featureA = E; \

528

featureB = E; \

529

}

530

531

void

532

EdgeEdgeTests(

533

bool & done,

534

float & minDistSqr,

535

vmPoint3 & localPointA,

536

vmPoint3 & localPointB,

537

int & otherFaceDimA,

538

int & otherFaceDimB,

539

FeatureType & featureA,

540

FeatureType & featureB,

541

const vmVector3 & hA,

542

const vmVector3 & hB,

543

PE_REF(vmVector3) faceOffsetAB,

544

PE_REF(vmVector3) faceOffsetBA,

545

const vmMatrix3 & matrixAB,

546

const vmMatrix3 & matrixBA,

547

PE_REF(vmVector3) signsA,

548

PE_REF(vmVector3) signsB,

549

PE_REF(vmVector3) scalesA,

550

PE_REF(vmVector3) scalesB,

551

bool first )

552

{

553

554

float distSqr;

555

float tA, tB;

556

557

int testOtherFaceDimA, testOtherFaceDimB;

558

559

testOtherFaceDimA = 0;

560

testOtherFaceDimB = 0;

561

562

distSqr = CustomEdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,

563

matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );

564

565

if ( first ) {

566

EdgeEdge_SetNewMin( X, Y, X, Y );

567

} else {

568

if ( distSqr < minDistSqr ) {

569

EdgeEdge_SetNewMin( X, Y, X, Y );

570

}

571

}

572

573

if ( done )

574

return;

575

576

signsA.setX( -signsA.getX() );

577

scalesA.setX( -scalesA.getX() );

578

579

distSqr = CustomEdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,

580

matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );

581

582

if ( distSqr < minDistSqr ) {

583

EdgeEdge_SetNewMin( X, Y, X, Y );

584

}

585

586

if ( done )

587

return;

588

589

signsB.setX( -signsB.getX() );

590

scalesB.setX( -scalesB.getX() );

591

592

distSqr = CustomEdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,

593

matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );

594

595

if ( distSqr < minDistSqr ) {

596

EdgeEdge_SetNewMin( X, Y, X, Y );

597

}

598

599

if ( done )

600

return;

601

602

signsA.setX( -signsA.getX() );

603

scalesA.setX( -scalesA.getX() );

604

605

distSqr = CustomEdgeEdgeTest_0101( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,

606

matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );

607

608

if ( distSqr < minDistSqr ) {

609

EdgeEdge_SetNewMin( X, Y, X, Y );

610

}

611

612

if ( done )

613

return;

614

615

testOtherFaceDimA = 1;

616

testOtherFaceDimB = 0;

617

signsB.setX( -signsB.getX() );

618

scalesB.setX( -scalesB.getX() );

619

620

distSqr = CustomEdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,

621

matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );

622

623

if ( distSqr < minDistSqr ) {

624

EdgeEdge_SetNewMin( Y, X, X, Y );

625

}

626

627

if ( done )

628

return;

629

630

signsA.setY( -signsA.getY() );

631

scalesA.setY( -scalesA.getY() );

632

633

distSqr = CustomEdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,

634

matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );

635

636

if ( distSqr < minDistSqr ) {

637

EdgeEdge_SetNewMin( Y, X, X, Y );

638

}

639

640

if ( done )

641

return;

642

643

signsB.setX( -signsB.getX() );

644

scalesB.setX( -scalesB.getX() );

645

646

distSqr = CustomEdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,

647

matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );

648

649

if ( distSqr < minDistSqr ) {

650

EdgeEdge_SetNewMin( Y, X, X, Y );

651

}

652

653

if ( done )

654

return;

655

656

signsA.setY( -signsA.getY() );

657

scalesA.setY( -scalesA.getY() );

658

659

distSqr = CustomEdgeEdgeTest_1001( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,

660

matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );

661

662

if ( distSqr < minDistSqr ) {

663

EdgeEdge_SetNewMin( Y, X, X, Y );

664

}

665

666

if ( done )

667

return;

668

669

testOtherFaceDimA = 0;

670

testOtherFaceDimB = 1;

671

signsB.setX( -signsB.getX() );

672

scalesB.setX( -scalesB.getX() );

673

674

distSqr = CustomEdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,

675

matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );

676

677

if ( distSqr < minDistSqr ) {

678

EdgeEdge_SetNewMin( X, Y, Y, X );

679

}

680

681

if ( done )

682

return;

683

684

signsA.setX( -signsA.getX() );

685

scalesA.setX( -scalesA.getX() );

686

687

distSqr = CustomEdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,

688

matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );

689

690

if ( distSqr < minDistSqr ) {

691

EdgeEdge_SetNewMin( X, Y, Y, X );

692

}

693

694

if ( done )

695

return;

696

697

signsB.setY( -signsB.getY() );

698

scalesB.setY( -scalesB.getY() );

699

700

distSqr = CustomEdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,

701

matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );

702

703

if ( distSqr < minDistSqr ) {

704

EdgeEdge_SetNewMin( X, Y, Y, X );

705

}

706

707

if ( done )

708

return;

709

710

signsA.setX( -signsA.getX() );

711

scalesA.setX( -scalesA.getX() );

712

713

distSqr = CustomEdgeEdgeTest_0110( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,

714

matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );

715

716

if ( distSqr < minDistSqr ) {

717

EdgeEdge_SetNewMin( X, Y, Y, X );

718

}

719

720

if ( done )

721

return;

722

723

testOtherFaceDimA = 1;

724

testOtherFaceDimB = 1;

725

signsB.setY( -signsB.getY() );

726

scalesB.setY( -scalesB.getY() );

727

728

distSqr = CustomEdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,

729

matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );

730

731

if ( distSqr < minDistSqr ) {

732

EdgeEdge_SetNewMin( Y, X, Y, X );

733

}

734

735

if ( done )

736

return;

737

738

signsA.setY( -signsA.getY() );

739

scalesA.setY( -scalesA.getY() );

740

741

distSqr = CustomEdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,

742

matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );

743

744

if ( distSqr < minDistSqr ) {

745

EdgeEdge_SetNewMin( Y, X, Y, X );

746

}

747

748

if ( done )

749

return;

750

751

signsB.setY( -signsB.getY() );

752

scalesB.setY( -scalesB.getY() );

753

754

distSqr = CustomEdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,

755

matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );

756

757

if ( distSqr < minDistSqr ) {

758

EdgeEdge_SetNewMin( Y, X, Y, X );

759

}

760

761

if ( done )

762

return;

763

764

signsA.setY( -signsA.getY() );

765

scalesA.setY( -scalesA.getY() );

766

767

distSqr = CustomEdgeEdgeTest_1010( done, tA, tB, hA, hB, faceOffsetAB, faceOffsetBA,

768

matrixAB, matrixBA, signsA, signsB, scalesA, scalesB );

769

770

if ( distSqr < minDistSqr ) {

771

EdgeEdge_SetNewMin( Y, X, Y, X );

772

}

773

}

774

775

776

float

777

boxBoxDistance(vmVector3& normal, BoxPoint& boxPointA, BoxPoint& boxPointB,

778

PE_REF(Box) boxA, const vmTransform3 & transformA, PE_REF(Box) boxB,

779

const vmTransform3 & transformB,

780

float distanceThreshold)

781

{

782

vmMatrix3 identity;

783

identity = vmMatrix3::identity();

784

vmVector3 ident[3];

785

ident[0] = identity.getCol0();

786

ident[1] = identity.getCol1();

787

ident[2] = identity.getCol2();

788

789

// get relative transformations

790

791

vmTransform3 transformAB, transformBA;

792

vmMatrix3 matrixAB, matrixBA;

793

vmVector3 offsetAB, offsetBA;

794

795

transformAB = orthoInverse(transformA) * transformB;

796

transformBA = orthoInverse(transformAB);

797

798

matrixAB = transformAB.getUpper3x3();

799

offsetAB = transformAB.getTranslation();

800

matrixBA = transformBA.getUpper3x3();

801

offsetBA = transformBA.getTranslation();

802

803

vmMatrix3 absMatrixAB = absPerElem(matrixAB);

804

vmMatrix3 absMatrixBA = absPerElem(matrixBA);

805

806

// find separating axis with largest gap between projections

807

808

BoxSepAxisType axisType;

809

vmVector3 axisA(0.0f), axisB(0.0f);

810

float gap, maxGap;

811

int faceDimA = 0, faceDimB = 0, edgeDimA = 0, edgeDimB = 0;

812

813

// face axes

814

815

vmVector3 gapsA = absPerElem(offsetAB) - boxA.mHalf - absMatrixAB * boxB.mHalf;

816

817

AaxisTest(0,X,true);

818

AaxisTest(1,Y,false);

819

AaxisTest(2,Z,false);

820

821

vmVector3 gapsB = absPerElem(offsetBA) - boxB.mHalf - absMatrixBA * boxA.mHalf;

822

823

BaxisTest(0,X);

824

BaxisTest(1,Y);

825

BaxisTest(2,Z);

826

827

// cross product axes

828

829

// �O�ς��O�̂Ƃ��̑΍�

830

absMatrixAB += vmMatrix3(1.0e-5f);

831

absMatrixBA += vmMatrix3(1.0e-5f);

832

833

vmMatrix3 lsqrs, projOffset, projAhalf, projBhalf;

834

835

lsqrs.setCol0( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) +

836

mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) );

837

lsqrs.setCol1( mulPerElem( matrixBA.getCol2(), matrixBA.getCol2() ) +

838

mulPerElem( matrixBA.getCol0(), matrixBA.getCol0() ) );

839

lsqrs.setCol2( mulPerElem( matrixBA.getCol1(), matrixBA.getCol1() ) +

840

mulPerElem( matrixBA.getCol0(), matrixBA.getCol0() ) );

841

842

projOffset.setCol0(matrixBA.getCol1() * offsetAB.getZ() - matrixBA.getCol2() * offsetAB.getY());

843

projOffset.setCol1(matrixBA.getCol2() * offsetAB.getX() - matrixBA.getCol0() * offsetAB.getZ());

844

projOffset.setCol2(matrixBA.getCol0() * offsetAB.getY() - matrixBA.getCol1() * offsetAB.getX());

845

846

projAhalf.setCol0(absMatrixBA.getCol1() * boxA.mHalf.getZ() + absMatrixBA.getCol2() * boxA.mHalf.getY());

847

projAhalf.setCol1(absMatrixBA.getCol2() * boxA.mHalf.getX() + absMatrixBA.getCol0() * boxA.mHalf.getZ());

848

projAhalf.setCol2(absMatrixBA.getCol0() * boxA.mHalf.getY() + absMatrixBA.getCol1() * boxA.mHalf.getX());

849

850

projBhalf.setCol0(absMatrixAB.getCol1() * boxB.mHalf.getZ() + absMatrixAB.getCol2() * boxB.mHalf.getY());

851

projBhalf.setCol1(absMatrixAB.getCol2() * boxB.mHalf.getX() + absMatrixAB.getCol0() * boxB.mHalf.getZ());

852

projBhalf.setCol2(absMatrixAB.getCol0() * boxB.mHalf.getY() + absMatrixAB.getCol1() * boxB.mHalf.getX());

853

854

vmMatrix3 gapsAxB = absPerElem(projOffset) - projAhalf - transpose(projBhalf);

855

856

CrossAxisTest(0,0,X);

857

CrossAxisTest(0,1,Y);

858

CrossAxisTest(0,2,Z);

859

CrossAxisTest(1,0,X);

860

CrossAxisTest(1,1,Y);

861

CrossAxisTest(1,2,Z);

862

CrossAxisTest(2,0,X);

863

CrossAxisTest(2,1,Y);

864

CrossAxisTest(2,2,Z);

865

866

// need to pick the face on each box whose normal best matches the separating axis.

867

// will transform vectors to be in the coordinate system of this face to simplify things later.

868

// for this, a permutation matrix can be used, which the next section computes.

869

870

int dimA[3], dimB[3];

871

872

if ( axisType == A_AXIS ) {

873

if ( dot(axisA,offsetAB) < 0.0f )

874

axisA = -axisA;

875

axisB = matrixBA * -axisA;

876

877

vmVector3 absAxisB = vmVector3(absPerElem(axisB));

878

879

if ( ( absAxisB[0] > absAxisB[1] ) && ( absAxisB[0] > absAxisB[2] ) )

880

faceDimB = 0;

881

else if ( absAxisB[1] > absAxisB[2] )

882

faceDimB = 1;

883

else

884

faceDimB = 2;

885

} else if ( axisType == B_AXIS ) {

886

if ( dot(axisB,offsetBA) < 0.0f )

887

axisB = -axisB;

888

axisA = matrixAB * -axisB;

889

890

vmVector3 absAxisA = vmVector3(absPerElem(axisA));

891

892

if ( ( absAxisA[0] > absAxisA[1] ) && ( absAxisA[0] > absAxisA[2] ) )

893

faceDimA = 0;

894

else if ( absAxisA[1] > absAxisA[2] )

895

faceDimA = 1;

896

else

897

faceDimA = 2;

898

}

899

900

if ( axisType == CROSS_AXIS ) {

901

if ( dot(axisA,offsetAB) < 0.0f )

902

axisA = -axisA;

903

axisB = matrixBA * -axisA;

904

905

vmVector3 absAxisA = vmVector3(absPerElem(axisA));

906

vmVector3 absAxisB = vmVector3(absPerElem(axisB));

907

908

dimA[1] = edgeDimA;

909

dimB[1] = edgeDimB;

910

911

if ( edgeDimA == 0 ) {

912

if ( absAxisA[1] > absAxisA[2] ) {

913

dimA[0] = 2;

914

dimA[2] = 1;

915

} else {

916

dimA[0] = 1;

917

dimA[2] = 2;

918

}

919

} else if ( edgeDimA == 1 ) {

920

if ( absAxisA[2] > absAxisA[0] ) {

921

dimA[0] = 0;

922

dimA[2] = 2;

923

} else {

924

dimA[0] = 2;

925

dimA[2] = 0;

926

}

927

} else {

928

if ( absAxisA[0] > absAxisA[1] ) {

929

dimA[0] = 1;

930

dimA[2] = 0;

931

} else {

932

dimA[0] = 0;

933

dimA[2] = 1;

934

}

935

}

936

937

if ( edgeDimB == 0 ) {

938

if ( absAxisB[1] > absAxisB[2] ) {

939

dimB[0] = 2;

940

dimB[2] = 1;

941

} else {

942

dimB[0] = 1;

943

dimB[2] = 2;

944

}

945

} else if ( edgeDimB == 1 ) {

946

if ( absAxisB[2] > absAxisB[0] ) {

947

dimB[0] = 0;

948

dimB[2] = 2;

949

} else {

950

dimB[0] = 2;

951

dimB[2] = 0;

952

}

953

} else {

954

if ( absAxisB[0] > absAxisB[1] ) {

955

dimB[0] = 1;

956

dimB[2] = 0;

957

} else {

958

dimB[0] = 0;

959

dimB[2] = 1;

960

}

961

}

962

} else {

963

dimA[2] = faceDimA;

964

dimA[0] = (faceDimA+1)%3;

965

dimA[1] = (faceDimA+2)%3;

966

dimB[2] = faceDimB;

967

dimB[0] = (faceDimB+1)%3;

968

dimB[1] = (faceDimB+2)%3;

969

}

970

971

vmMatrix3 aperm_col, bperm_col;

972

973

aperm_col.setCol0(ident[dimA[0]]);

974

aperm_col.setCol1(ident[dimA[1]]);

975

aperm_col.setCol2(ident[dimA[2]]);

976

977

bperm_col.setCol0(ident[dimB[0]]);

978

bperm_col.setCol1(ident[dimB[1]]);

979

bperm_col.setCol2(ident[dimB[2]]);

980

981

vmMatrix3 aperm_row, bperm_row;

982

983

aperm_row = transpose(aperm_col);

984

bperm_row = transpose(bperm_col);

985

986

// permute all box parameters to be in the face coordinate systems

987

988

vmMatrix3 matrixAB_perm = aperm_row * matrixAB * bperm_col;

989

vmMatrix3 matrixBA_perm = transpose(matrixAB_perm);

990

991

vmVector3 offsetAB_perm, offsetBA_perm;

992

993

offsetAB_perm = aperm_row * offsetAB;

994

offsetBA_perm = bperm_row * offsetBA;

995

996

vmVector3 halfA_perm, halfB_perm;

997

998

halfA_perm = aperm_row * boxA.mHalf;

999

halfB_perm = bperm_row * boxB.mHalf;

1000

1001

// compute the vector between the centers of each face, in each face's coordinate frame

1002

1003

vmVector3 signsA_perm, signsB_perm, scalesA_perm, scalesB_perm, faceOffsetAB_perm, faceOffsetBA_perm;

1004

1005

signsA_perm = copySignPerElem(vmVector3(1.0f),aperm_row * axisA);

1006

signsB_perm = copySignPerElem(vmVector3(1.0f),bperm_row * axisB);

1007

scalesA_perm = mulPerElem( signsA_perm, halfA_perm );

1008

scalesB_perm = mulPerElem( signsB_perm, halfB_perm );

1009

1010

faceOffsetAB_perm = offsetAB_perm + matrixAB_perm.getCol2() * scalesB_perm.getZ();

1011

faceOffsetAB_perm.setZ( faceOffsetAB_perm.getZ() - scalesA_perm.getZ() );

1012

1013

faceOffsetBA_perm = offsetBA_perm + matrixBA_perm.getCol2() * scalesA_perm.getZ();

1014

faceOffsetBA_perm.setZ( faceOffsetBA_perm.getZ() - scalesB_perm.getZ() );

1015

1016

if ( maxGap < 0.0f ) {

1017

// if boxes overlap, this will separate the faces for finding points of penetration.

1018

1019

faceOffsetAB_perm -= aperm_row * axisA * maxGap * 1.01f;

1020

faceOffsetBA_perm -= bperm_row * axisB * maxGap * 1.01f;

1021

}

1022

1023

// for each vertex/face or edge/edge pair of the two faces, find the closest points.

1024

1025

// these points each have an associated box feature (vertex, edge, or face). if each

1026

// point is in the external Voronoi region of the other's feature, they are the

1027

// closest points of the boxes, and the algorithm can exit.

1028

1029

// the feature pairs are arranged so that in the general case, the first test will

1030

// succeed. degenerate cases (parallel faces) may require up to all tests in the

1031

// worst case.

1032

1033

// if for some reason no case passes the Voronoi test, the features with the minimum

1034

// distance are returned.

1035

1036

vmPoint3 localPointA_perm, localPointB_perm;

1037

float minDistSqr;

1038

bool done;

1039

1040

vmVector3 hA_perm( halfA_perm ), hB_perm( halfB_perm );

1041

1042

localPointA_perm.setZ( scalesA_perm.getZ() );

1043

localPointB_perm.setZ( scalesB_perm.getZ() );

1044

scalesA_perm.setZ(0.0f);

1045

scalesB_perm.setZ(0.0f);

1046

1047

int otherFaceDimA, otherFaceDimB;

1048

FeatureType featureA, featureB;

1049

1050

if ( axisType == CROSS_AXIS ) {

1051

EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm,

1052

otherFaceDimA, otherFaceDimB, featureA, featureB,

1053

hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,

1054

matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm,

1055

scalesA_perm, scalesB_perm, true );

1056

1057

if ( !done ) {

1058

VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm,

1059

featureA, featureB,

1060

hA_perm, faceOffsetAB_perm, faceOffsetBA_perm,

1061

matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, false );

1062

1063

if ( !done ) {

1064

VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm,

1065

featureA, featureB,

1066

hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,

1067

matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, false );

1068

}

1069

}

1070

} else if ( axisType == B_AXIS ) {

1071

VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm,

1072

featureA, featureB,

1073

hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,

1074

matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, true );

1075

1076

if ( !done ) {

1077

VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm,

1078

featureA, featureB,

1079

hA_perm, faceOffsetAB_perm, faceOffsetBA_perm,

1080

matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, false );

1081

1082

if ( !done ) {

1083

EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm,

1084

otherFaceDimA, otherFaceDimB, featureA, featureB,

1085

hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,

1086

matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm,

1087

scalesA_perm, scalesB_perm, false );

1088

}

1089

}

1090

} else {

1091

VertexBFaceATests( done, minDistSqr, localPointA_perm, localPointB_perm,

1092

featureA, featureB,

1093

hA_perm, faceOffsetAB_perm, faceOffsetBA_perm,

1094

matrixAB_perm, matrixBA_perm, signsB_perm, scalesB_perm, true );

1095

1096

if ( !done ) {

1097

VertexAFaceBTests( done, minDistSqr, localPointA_perm, localPointB_perm,

1098

featureA, featureB,

1099

hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,

1100

matrixAB_perm, matrixBA_perm, signsA_perm, scalesA_perm, false );

1101

1102

if ( !done ) {

1103

EdgeEdgeTests( done, minDistSqr, localPointA_perm, localPointB_perm,

1104

otherFaceDimA, otherFaceDimB, featureA, featureB,

1105

hA_perm, hB_perm, faceOffsetAB_perm, faceOffsetBA_perm,

1106

matrixAB_perm, matrixBA_perm, signsA_perm, signsB_perm,

1107

scalesA_perm, scalesB_perm, false );

1108

}

1109

}

1110

}

1111

1112

// convert local points from face-local to box-local coordinate system

1113

1114

1115

boxPointA.localPoint = vmPoint3( aperm_col * vmVector3( localPointA_perm )) ;

1116

boxPointB.localPoint = vmPoint3( bperm_col * vmVector3( localPointB_perm )) ;

1117

1118

#if 0

1119

// find which features of the boxes are involved.

1120

// the only feature pairs which occur in this function are VF, FV, and EE, even though the

1121

// closest points might actually lie on sub-features, as in a VF contact might be used for

1122

// what's actually a VV contact. this means some feature pairs could possibly seem distinct

1123

// from others, although their contact positions are the same. don't know yet whether this

1124

// matters.

1125

1126

int sA[3], sB[3];

1127

1128

sA[0] = boxPointA.localPoint.getX() > 0.0f;

1129

sA[1] = boxPointA.localPoint.getY() > 0.0f;

1130

sA[2] = boxPointA.localPoint.getZ() > 0.0f;

1131

1132

sB[0] = boxPointB.localPoint.getX() > 0.0f;

1133

sB[1] = boxPointB.localPoint.getY() > 0.0f;

1134

sB[2] = boxPointB.localPoint.getZ() > 0.0f;

1135

1136

if ( featureA == F ) {

1137

boxPointA.setFaceFeature( dimA[2], sA[dimA[2]] );

1138

} else if ( featureA == E ) {

1139

boxPointA.setEdgeFeature( dimA[2], sA[dimA[2]], dimA[otherFaceDimA], sA[dimA[otherFaceDimA]] );

1140

} else {

1141

boxPointA.setVertexFeature( sA[0], sA[1], sA[2] );

1142

}

1143

1144

if ( featureB == F ) {

1145

boxPointB.setFaceFeature( dimB[2], sB[dimB[2]] );

1146

} else if ( featureB == E ) {

1147

boxPointB.setEdgeFeature( dimB[2], sB[dimB[2]], dimB[otherFaceDimB], sB[dimB[otherFaceDimB]] );

1148

} else {

1149

boxPointB.setVertexFeature( sB[0], sB[1], sB[2] );

1150

}

1151

#endif

1152

1153

normal = transformA * axisA;

1154

1155

if ( maxGap < 0.0f ) {

1156

return (maxGap);

1157

} else {

1158

return (sqrtf( minDistSqr ));

1159

}

1160

}

Older »