~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.

2007-09-09

Refactored by Francisco Le?n

email: projectileman@yahoo.com

http://gimpact.sf.net

#include "btGeneric6DofConstraint.h"

#include "BulletDynamics/Dynamics/btRigidBody.h"

#include "LinearMath/btTransformUtil.h"

#include <new>

#define D6_USE_OBSOLETE_METHOD false

#define D6_USE_FRAME_OFFSET true

btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbA, btRigidBody& rbB, const btTransform& frameInA, const btTransform& frameInB, bool useLinearReferenceFrameA)

: btTypedConstraint(D6_CONSTRAINT_TYPE, rbA, rbB)

, m_frameInA(frameInA)

, m_frameInB(frameInB),

m_useLinearReferenceFrameA(useLinearReferenceFrameA),

m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),

m_flags(0),

m_useSolveConstraintObsolete(D6_USE_OBSOLETE_METHOD)

{

calculateTransforms();

}

btGeneric6DofConstraint::btGeneric6DofConstraint(btRigidBody& rbB, const btTransform& frameInB, bool useLinearReferenceFrameB)

: btTypedConstraint(D6_CONSTRAINT_TYPE, getFixedBody(), rbB),

m_frameInB(frameInB),

m_useLinearReferenceFrameA(useLinearReferenceFrameB),

m_useOffsetForConstraintFrame(D6_USE_FRAME_OFFSET),

m_flags(0),

m_useSolveConstraintObsolete(false)

{

///not providing rigidbody A means implicitly using worldspace for body A

m_frameInA = rbB.getCenterOfMassTransform() * m_frameInB;

calculateTransforms();

}

#define GENERIC_D6_DISABLE_WARMSTARTING 1

btScalar btGetMatrixElem(const btMatrix3x3& mat, int index);

btScalar btGetMatrixElem(const btMatrix3x3& mat, int index)

{

int i = index%3;

int j = index/3;

return mat[i][j];

}

///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html

bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz);

bool matrixToEulerXYZ(const btMatrix3x3& mat,btVector3& xyz)

{

// // rot = cy*cz -cy*sz sy

// // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx

// // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy

btScalar fi = btGetMatrixElem(mat,2);

if (fi < btScalar(1.0f))

{

if (fi > btScalar(-1.0f))

{

xyz[0] = btAtan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));

xyz[1] = btAsin(btGetMatrixElem(mat,2));

xyz[2] = btAtan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));

return true;

100

}

101

else

102

{

103

// WARNING. Not unique. XA - ZA = -atan2(r10,r11)

104

xyz[0] = -btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));

105

xyz[1] = -SIMD_HALF_PI;

106

xyz[2] = btScalar(0.0);

107

return false;

108

}

109

}

110

else

111

{

112

// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)

113

xyz[0] = btAtan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));

114

xyz[1] = SIMD_HALF_PI;

115

xyz[2] = 0.0;

116

}

117

return false;

118

}

119

120

//////////////////////////// btRotationalLimitMotor ////////////////////////////////////

121

122

int btRotationalLimitMotor::testLimitValue(btScalar test_value)

123

{

124

if(m_loLimit>m_hiLimit)

125

{

126

m_currentLimit = 0;//Free from violation

127

return 0;

128

}

129

if (test_value < m_loLimit)

130

{

131

m_currentLimit = 1;//low limit violation

132

m_currentLimitError = test_value - m_loLimit;

133

return 1;

134

}

135

else if (test_value> m_hiLimit)

136

{

137

m_currentLimit = 2;//High limit violation

138

m_currentLimitError = test_value - m_hiLimit;

139

return 2;

140

};

141

142

m_currentLimit = 0;//Free from violation

143

return 0;

144

145

}

146

147

148

149

btScalar btRotationalLimitMotor::solveAngularLimits(

150

btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,

151

btRigidBody * body0, btRigidBody * body1 )

152

{

153

if (needApplyTorques()==false) return 0.0f;

154

155

btScalar target_velocity = m_targetVelocity;

156

btScalar maxMotorForce = m_maxMotorForce;

157

158

//current error correction

159

if (m_currentLimit!=0)

160

{

161

target_velocity = -m_stopERP*m_currentLimitError/(timeStep);

162

maxMotorForce = m_maxLimitForce;

163

}

164

165

maxMotorForce *= timeStep;

166

167

// current velocity difference

168

169

btVector3 angVelA;

170

body0->internalGetAngularVelocity(angVelA);

171

btVector3 angVelB;

172

body1->internalGetAngularVelocity(angVelB);

173

174

btVector3 vel_diff;

175

vel_diff = angVelA-angVelB;

176

177

178

179

btScalar rel_vel = axis.dot(vel_diff);

180

181

// correction velocity

182

btScalar motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel);

183

184

185

if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON )

186

{

187

return 0.0f;//no need for applying force

188

}

189

190

191

// correction impulse

192

btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;

193

194

// clip correction impulse

195

btScalar clippedMotorImpulse;

196

197

///@todo: should clip against accumulated impulse

198

if (unclippedMotorImpulse>0.0f)

199

{

200

clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;

201

}

202

else

203

{

204

clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;

205

}

206

207

208

// sort with accumulated impulses

209

btScalar lo = btScalar(-BT_LARGE_FLOAT);

210

btScalar hi = btScalar(BT_LARGE_FLOAT);

211

212

btScalar oldaccumImpulse = m_accumulatedImpulse;

213

btScalar sum = oldaccumImpulse + clippedMotorImpulse;

214

m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;

215

216

clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;

217

218

btVector3 motorImp = clippedMotorImpulse * axis;

219

220

//body0->applyTorqueImpulse(motorImp);

221

//body1->applyTorqueImpulse(-motorImp);

222

223

body0->internalApplyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);

224

body1->internalApplyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);

225

226

227

return clippedMotorImpulse;

228

229

230

}

231

232

//////////////////////////// End btRotationalLimitMotor ////////////////////////////////////

233

234

235

236

237

//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////

238

239

240

int btTranslationalLimitMotor::testLimitValue(int limitIndex, btScalar test_value)

241

{

242

btScalar loLimit = m_lowerLimit[limitIndex];

243

btScalar hiLimit = m_upperLimit[limitIndex];

244

if(loLimit > hiLimit)

245

{

246

m_currentLimit[limitIndex] = 0;//Free from violation

247

m_currentLimitError[limitIndex] = btScalar(0.f);

248

return 0;

249

}

250

251

if (test_value < loLimit)

252

{

253

m_currentLimit[limitIndex] = 2;//low limit violation

254

m_currentLimitError[limitIndex] = test_value - loLimit;

255

return 2;

256

}

257

else if (test_value> hiLimit)

258

{

259

m_currentLimit[limitIndex] = 1;//High limit violation

260

m_currentLimitError[limitIndex] = test_value - hiLimit;

261

return 1;

262

};

263

264

m_currentLimit[limitIndex] = 0;//Free from violation

265

m_currentLimitError[limitIndex] = btScalar(0.f);

266

return 0;

267

}

268

269

270

271

btScalar btTranslationalLimitMotor::solveLinearAxis(

272

btScalar timeStep,

273

btScalar jacDiagABInv,

274

btRigidBody& body1,const btVector3 &pointInA,

275

btRigidBody& body2,const btVector3 &pointInB,

276

int limit_index,

277

const btVector3 & axis_normal_on_a,

278

const btVector3 & anchorPos)

279

{

280

281

///find relative velocity

282

// btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();

283

// btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();

284

btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();

285

btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();

286

287

btVector3 vel1;

288

body1.internalGetVelocityInLocalPointObsolete(rel_pos1,vel1);

289

btVector3 vel2;

290

body2.internalGetVelocityInLocalPointObsolete(rel_pos2,vel2);

291

btVector3 vel = vel1 - vel2;

292

293

btScalar rel_vel = axis_normal_on_a.dot(vel);

294

295

296

297

/// apply displacement correction

298

299

//positional error (zeroth order error)

300

btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);

301

btScalar lo = btScalar(-BT_LARGE_FLOAT);

302

btScalar hi = btScalar(BT_LARGE_FLOAT);

303

304

btScalar minLimit = m_lowerLimit[limit_index];

305

btScalar maxLimit = m_upperLimit[limit_index];

306

307

//handle the limits

308

if (minLimit < maxLimit)

309

{

310

{

311

if (depth > maxLimit)

312

{

313

depth -= maxLimit;

314

lo = btScalar(0.);

315

316

}

317

else

318

{

319

if (depth < minLimit)

320

{

321

depth -= minLimit;

322

hi = btScalar(0.);

323

}

324

else

325

{

326

return 0.0f;

327

}

328

}

329

}

330

}

331

332

btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;

333

334

335

336

337

btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];

338

btScalar sum = oldNormalImpulse + normalImpulse;

339

m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;

340

normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;

341

342

btVector3 impulse_vector = axis_normal_on_a * normalImpulse;

343

//body1.applyImpulse( impulse_vector, rel_pos1);

344

//body2.applyImpulse(-impulse_vector, rel_pos2);

345

346

btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);

347

btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);

348

body1.internalApplyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);

349

body2.internalApplyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);

350

351

352

353

354

return normalImpulse;

355

}

356

357

//////////////////////////// btTranslationalLimitMotor ////////////////////////////////////

358

359

void btGeneric6DofConstraint::calculateAngleInfo()

360

{

361

btMatrix3x3 relative_frame = m_calculatedTransformA.getBasis().inverse()*m_calculatedTransformB.getBasis();

362

matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);

363

// in euler angle mode we do not actually constrain the angular velocity

364

// along the axes axis[0] and axis[2] (although we do use axis[1]) :

365

366

// to get constrain w2-w1 along ...not

367

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

368

// d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]

369

// d(angle[1])/dt = 0 ax[1]

370

// d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]

371

372

// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.

373

// to prove the result for angle[0], write the expression for angle[0] from

374

// GetInfo1 then take the derivative. to prove this for angle[2] it is

375

// easier to take the euler rate expression for d(angle[2])/dt with respect

376

// to the components of w and set that to 0.

377

btVector3 axis0 = m_calculatedTransformB.getBasis().getColumn(0);

378

btVector3 axis2 = m_calculatedTransformA.getBasis().getColumn(2);

379

380

m_calculatedAxis[1] = axis2.cross(axis0);

381

m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);

382

m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);

383

384

m_calculatedAxis[0].normalize();

385

m_calculatedAxis[1].normalize();

386

m_calculatedAxis[2].normalize();

387

388

}

389

390

void btGeneric6DofConstraint::calculateTransforms()

391

{

392

calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());

393

}

394

395

void btGeneric6DofConstraint::calculateTransforms(const btTransform& transA,const btTransform& transB)

396

{

397

m_calculatedTransformA = transA * m_frameInA;

398

m_calculatedTransformB = transB * m_frameInB;

399

calculateLinearInfo();

400

calculateAngleInfo();

401

if(m_useOffsetForConstraintFrame)

402

{ // get weight factors depending on masses

403

btScalar miA = getRigidBodyA().getInvMass();

404

btScalar miB = getRigidBodyB().getInvMass();

405

m_hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);

406

btScalar miS = miA + miB;

407

if(miS > btScalar(0.f))

408

{

409

m_factA = miB / miS;

410

}

411

else

412

{

413

m_factA = btScalar(0.5f);

414

}

415

m_factB = btScalar(1.0f) - m_factA;

416

}

417

}

418

419

420

421

void btGeneric6DofConstraint::buildLinearJacobian(

422

btJacobianEntry & jacLinear,const btVector3 & normalWorld,

423

const btVector3 & pivotAInW,const btVector3 & pivotBInW)

424

{

425

new (&jacLinear) btJacobianEntry(

426

m_rbA.getCenterOfMassTransform().getBasis().transpose(),

427

m_rbB.getCenterOfMassTransform().getBasis().transpose(),

428

pivotAInW - m_rbA.getCenterOfMassPosition(),

429

pivotBInW - m_rbB.getCenterOfMassPosition(),

430

normalWorld,

431

m_rbA.getInvInertiaDiagLocal(),

432

m_rbA.getInvMass(),

433

m_rbB.getInvInertiaDiagLocal(),

434

m_rbB.getInvMass());

435

}

436

437

438

439

void btGeneric6DofConstraint::buildAngularJacobian(

440

btJacobianEntry & jacAngular,const btVector3 & jointAxisW)

441

{

442

new (&jacAngular) btJacobianEntry(jointAxisW,

443

m_rbA.getCenterOfMassTransform().getBasis().transpose(),

444

m_rbB.getCenterOfMassTransform().getBasis().transpose(),

445

m_rbA.getInvInertiaDiagLocal(),

446

m_rbB.getInvInertiaDiagLocal());

447

448

}

449

450

451

452

bool btGeneric6DofConstraint::testAngularLimitMotor(int axis_index)

453

{

454

btScalar angle = m_calculatedAxisAngleDiff[axis_index];

455

angle = btAdjustAngleToLimits(angle, m_angularLimits[axis_index].m_loLimit, m_angularLimits[axis_index].m_hiLimit);

456

m_angularLimits[axis_index].m_currentPosition = angle;

457

//test limits

458

m_angularLimits[axis_index].testLimitValue(angle);

459

return m_angularLimits[axis_index].needApplyTorques();

460

}

461

462

463

464

void btGeneric6DofConstraint::buildJacobian()

465

{

466

#ifndef __SPU__

467

if (m_useSolveConstraintObsolete)

468

{

469

470

// Clear accumulated impulses for the next simulation step

471

m_linearLimits.m_accumulatedImpulse.setValue(btScalar(0.), btScalar(0.), btScalar(0.));

472

int i;

473

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

474

{

475

m_angularLimits[i].m_accumulatedImpulse = btScalar(0.);

476

}

477

//calculates transform

478

calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());

479

480

// const btVector3& pivotAInW = m_calculatedTransformA.getOrigin();

481

// const btVector3& pivotBInW = m_calculatedTransformB.getOrigin();

482

calcAnchorPos();

483

btVector3 pivotAInW = m_AnchorPos;

484

btVector3 pivotBInW = m_AnchorPos;

485

486

// not used here

487

// btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();

488

// btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();

489

490

btVector3 normalWorld;

491

//linear part

492

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

493

{

494

if (m_linearLimits.isLimited(i))

495

{

496

if (m_useLinearReferenceFrameA)

497

normalWorld = m_calculatedTransformA.getBasis().getColumn(i);

498

else

499

normalWorld = m_calculatedTransformB.getBasis().getColumn(i);

500

501

buildLinearJacobian(

502

m_jacLinear[i],normalWorld ,

503

pivotAInW,pivotBInW);

504

505

}

506

}

507

508

// angular part

509

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

510

{

511

//calculates error angle

512

if (testAngularLimitMotor(i))

513

{

514

normalWorld = this->getAxis(i);

515

// Create angular atom

516

buildAngularJacobian(m_jacAng[i],normalWorld);

517

}

518

}

519

520

}

521

#endif //__SPU__

522

523

}

524

525

526

void btGeneric6DofConstraint::getInfo1 (btConstraintInfo1* info)

527

{

528

if (m_useSolveConstraintObsolete)

529

{

530

info->m_numConstraintRows = 0;

531

info->nub = 0;

532

} else

533

{

534

//prepare constraint

535

calculateTransforms(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());

536

info->m_numConstraintRows = 0;

537

info->nub = 6;

538

int i;

539

//test linear limits

540

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

541

{

542

if(m_linearLimits.needApplyForce(i))

543

{

544

info->m_numConstraintRows++;

545

info->nub--;

546

}

547

}

548

//test angular limits

549

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

550

{

551

if(testAngularLimitMotor(i))

552

{

553

info->m_numConstraintRows++;

554

info->nub--;

555

}

556

}

557

}

558

}

559

560

void btGeneric6DofConstraint::getInfo1NonVirtual (btConstraintInfo1* info)

561

{

562

if (m_useSolveConstraintObsolete)

563

{

564

info->m_numConstraintRows = 0;

565

info->nub = 0;

566

} else

567

{

568

//pre-allocate all 6

569

info->m_numConstraintRows = 6;

570

info->nub = 0;

571

}

572

}

573

574

575

void btGeneric6DofConstraint::getInfo2 (btConstraintInfo2* info)

576

{

577

btAssert(!m_useSolveConstraintObsolete);

578

579

const btTransform& transA = m_rbA.getCenterOfMassTransform();

580

const btTransform& transB = m_rbB.getCenterOfMassTransform();

581

const btVector3& linVelA = m_rbA.getLinearVelocity();

582

const btVector3& linVelB = m_rbB.getLinearVelocity();

583

const btVector3& angVelA = m_rbA.getAngularVelocity();

584

const btVector3& angVelB = m_rbB.getAngularVelocity();

585

586

if(m_useOffsetForConstraintFrame)

587

{ // for stability better to solve angular limits first

588

int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);

589

setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);

590

}

591

else

592

{ // leave old version for compatibility

593

int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);

594

setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);

595

}

596

597

}

598

599

600

void btGeneric6DofConstraint::getInfo2NonVirtual (btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)

601

{

602

603

btAssert(!m_useSolveConstraintObsolete);

604

//prepare constraint

605

calculateTransforms(transA,transB);

606

607

int i;

608

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

609

{

610

testAngularLimitMotor(i);

611

}

612

613

if(m_useOffsetForConstraintFrame)

614

{ // for stability better to solve angular limits first

615

int row = setAngularLimits(info, 0,transA,transB,linVelA,linVelB,angVelA,angVelB);

616

setLinearLimits(info, row, transA,transB,linVelA,linVelB,angVelA,angVelB);

617

}

618

else

619

{ // leave old version for compatibility

620

int row = setLinearLimits(info, 0, transA,transB,linVelA,linVelB,angVelA,angVelB);

621

setAngularLimits(info, row,transA,transB,linVelA,linVelB,angVelA,angVelB);

622

}

623

}

624

625

626

627

int btGeneric6DofConstraint::setLinearLimits(btConstraintInfo2* info, int row, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)

628

{

629

// int row = 0;

630

//solve linear limits

631

btRotationalLimitMotor limot;

632

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

633

{

634

if(m_linearLimits.needApplyForce(i))

635

{ // re-use rotational motor code

636

limot.m_bounce = btScalar(0.f);

637

limot.m_currentLimit = m_linearLimits.m_currentLimit[i];

638

limot.m_currentPosition = m_linearLimits.m_currentLinearDiff[i];

639

limot.m_currentLimitError = m_linearLimits.m_currentLimitError[i];

640

limot.m_damping = m_linearLimits.m_damping;

641

limot.m_enableMotor = m_linearLimits.m_enableMotor[i];

642

limot.m_hiLimit = m_linearLimits.m_upperLimit[i];

643

limot.m_limitSoftness = m_linearLimits.m_limitSoftness;

644

limot.m_loLimit = m_linearLimits.m_lowerLimit[i];

645

limot.m_maxLimitForce = btScalar(0.f);

646

limot.m_maxMotorForce = m_linearLimits.m_maxMotorForce[i];

647

limot.m_targetVelocity = m_linearLimits.m_targetVelocity[i];

648

btVector3 axis = m_calculatedTransformA.getBasis().getColumn(i);

649

int flags = m_flags >> (i * BT_6DOF_FLAGS_AXIS_SHIFT);

650

limot.m_normalCFM = (flags & BT_6DOF_FLAGS_CFM_NORM) ? m_linearLimits.m_normalCFM[i] : info->cfm[0];

651

limot.m_stopCFM = (flags & BT_6DOF_FLAGS_CFM_STOP) ? m_linearLimits.m_stopCFM[i] : info->cfm[0];

652

limot.m_stopERP = (flags & BT_6DOF_FLAGS_ERP_STOP) ? m_linearLimits.m_stopERP[i] : info->erp;

653

if(m_useOffsetForConstraintFrame)

654

{

655

int indx1 = (i + 1) % 3;

656

int indx2 = (i + 2) % 3;

657

int rotAllowed = 1; // rotations around orthos to current axis

658

if(m_angularLimits[indx1].m_currentLimit && m_angularLimits[indx2].m_currentLimit)

659

{

660

rotAllowed = 0;

661

}

662

row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0, rotAllowed);

663

}

664

else

665

{

666

row += get_limit_motor_info2(&limot, transA,transB,linVelA,linVelB,angVelA,angVelB, info, row, axis, 0);

667

}

668

}

669

}

670

return row;

671

}

672

673

674

675

int btGeneric6DofConstraint::setAngularLimits(btConstraintInfo2 *info, int row_offset, const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB)

676

{

677

btGeneric6DofConstraint * d6constraint = this;

678

int row = row_offset;

679

//solve angular limits

680

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

681

{

682

if(d6constraint->getRotationalLimitMotor(i)->needApplyTorques())

683

{

684

btVector3 axis = d6constraint->getAxis(i);

685

int flags = m_flags >> ((i + 3) * BT_6DOF_FLAGS_AXIS_SHIFT);

686

if(!(flags & BT_6DOF_FLAGS_CFM_NORM))

687

{

688

m_angularLimits[i].m_normalCFM = info->cfm[0];

689

}

690

if(!(flags & BT_6DOF_FLAGS_CFM_STOP))

691

{

692

m_angularLimits[i].m_stopCFM = info->cfm[0];

693

}

694

if(!(flags & BT_6DOF_FLAGS_ERP_STOP))

695

{

696

m_angularLimits[i].m_stopERP = info->erp;

697

}

698

row += get_limit_motor_info2(d6constraint->getRotationalLimitMotor(i),

699

transA,transB,linVelA,linVelB,angVelA,angVelB, info,row,axis,1);

700

}

701

}

702

703

return row;

704

}

705

706

707

708

709

void btGeneric6DofConstraint::updateRHS(btScalar timeStep)

710

{

711

(void)timeStep;

712

713

}

714

715

716

void btGeneric6DofConstraint::setFrames(const btTransform& frameA, const btTransform& frameB)

717

{

718

m_frameInA = frameA;

719

m_frameInB = frameB;

720

buildJacobian();

721

calculateTransforms();

722

}

723

724

725

726

btVector3 btGeneric6DofConstraint::getAxis(int axis_index) const

727

{

728

return m_calculatedAxis[axis_index];

729

}

730

731

732

btScalar btGeneric6DofConstraint::getRelativePivotPosition(int axisIndex) const

733

{

734

return m_calculatedLinearDiff[axisIndex];

735

}

736

737

738

btScalar btGeneric6DofConstraint::getAngle(int axisIndex) const

739

{

740

return m_calculatedAxisAngleDiff[axisIndex];

741

}

742

743

744

745

void btGeneric6DofConstraint::calcAnchorPos(void)

746

{

747

btScalar imA = m_rbA.getInvMass();

748

btScalar imB = m_rbB.getInvMass();

749

btScalar weight;

750

if(imB == btScalar(0.0))

751

{

752

weight = btScalar(1.0);

753

}

754

else

755

{

756

weight = imA / (imA + imB);

757

}

758

const btVector3& pA = m_calculatedTransformA.getOrigin();

759

const btVector3& pB = m_calculatedTransformB.getOrigin();

760

m_AnchorPos = pA * weight + pB * (btScalar(1.0) - weight);

761

return;

762

}

763

764

765

766

void btGeneric6DofConstraint::calculateLinearInfo()

767

{

768

m_calculatedLinearDiff = m_calculatedTransformB.getOrigin() - m_calculatedTransformA.getOrigin();

769

m_calculatedLinearDiff = m_calculatedTransformA.getBasis().inverse() * m_calculatedLinearDiff;

770

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

771

{

772

m_linearLimits.m_currentLinearDiff[i] = m_calculatedLinearDiff[i];

773

m_linearLimits.testLimitValue(i, m_calculatedLinearDiff[i]);

774

}

775

}

776

777

778

779

int btGeneric6DofConstraint::get_limit_motor_info2(

780

btRotationalLimitMotor * limot,

781

const btTransform& transA,const btTransform& transB,const btVector3& linVelA,const btVector3& linVelB,const btVector3& angVelA,const btVector3& angVelB,

782

btConstraintInfo2 *info, int row, btVector3& ax1, int rotational,int rotAllowed)

783

{

784

int srow = row * info->rowskip;

785

int powered = limot->m_enableMotor;

786

int limit = limot->m_currentLimit;

787

if (powered || limit)

788

{ // if the joint is powered, or has joint limits, add in the extra row

789

btScalar *J1 = rotational ? info->m_J1angularAxis : info->m_J1linearAxis;

790

btScalar *J2 = rotational ? info->m_J2angularAxis : 0;

791

J1[srow+0] = ax1[0];

792

J1[srow+1] = ax1[1];

793

J1[srow+2] = ax1[2];

794

if(rotational)

795

{

796

J2[srow+0] = -ax1[0];

797

J2[srow+1] = -ax1[1];

798

J2[srow+2] = -ax1[2];

799

}

800

if((!rotational))

801

{

802

if (m_useOffsetForConstraintFrame)

803

{

804

btVector3 tmpA, tmpB, relA, relB;

805

// get vector from bodyB to frameB in WCS

806

relB = m_calculatedTransformB.getOrigin() - transB.getOrigin();

807

// get its projection to constraint axis

808

btVector3 projB = ax1 * relB.dot(ax1);

809

// get vector directed from bodyB to constraint axis (and orthogonal to it)

810

btVector3 orthoB = relB - projB;

811

// same for bodyA

812

relA = m_calculatedTransformA.getOrigin() - transA.getOrigin();

813

btVector3 projA = ax1 * relA.dot(ax1);

814

btVector3 orthoA = relA - projA;

815

// get desired offset between frames A and B along constraint axis

816

btScalar desiredOffs = limot->m_currentPosition - limot->m_currentLimitError;

817

// desired vector from projection of center of bodyA to projection of center of bodyB to constraint axis

818

btVector3 totalDist = projA + ax1 * desiredOffs - projB;

819

// get offset vectors relA and relB

820

relA = orthoA + totalDist * m_factA;

821

relB = orthoB - totalDist * m_factB;

822

tmpA = relA.cross(ax1);

823

tmpB = relB.cross(ax1);

824

if(m_hasStaticBody && (!rotAllowed))

825

{

826

tmpA *= m_factA;

827

tmpB *= m_factB;

828

}

829

int i;

830

for (i=0; i<3; i++) info->m_J1angularAxis[srow+i] = tmpA[i];

831

for (i=0; i<3; i++) info->m_J2angularAxis[srow+i] = -tmpB[i];

832

} else

833

{

834

btVector3 ltd; // Linear Torque Decoupling vector

835

btVector3 c = m_calculatedTransformB.getOrigin() - transA.getOrigin();

836

ltd = c.cross(ax1);

837

info->m_J1angularAxis[srow+0] = ltd[0];

838

info->m_J1angularAxis[srow+1] = ltd[1];

839

info->m_J1angularAxis[srow+2] = ltd[2];

840

841

c = m_calculatedTransformB.getOrigin() - transB.getOrigin();

842

ltd = -c.cross(ax1);

843

info->m_J2angularAxis[srow+0] = ltd[0];

844

info->m_J2angularAxis[srow+1] = ltd[1];

845

info->m_J2angularAxis[srow+2] = ltd[2];

846

}

847

}

848

// if we're limited low and high simultaneously, the joint motor is

849

// ineffective

850

if (limit && (limot->m_loLimit == limot->m_hiLimit)) powered = 0;

851

info->m_constraintError[srow] = btScalar(0.f);

852

if (powered)

853

{

854

info->cfm[srow] = limot->m_normalCFM;

855

if(!limit)

856

{

857

btScalar tag_vel = rotational ? limot->m_targetVelocity : -limot->m_targetVelocity;

858

859

btScalar mot_fact = getMotorFactor( limot->m_currentPosition,

860

limot->m_loLimit,

861

limot->m_hiLimit,

862

tag_vel,

863

info->fps * limot->m_stopERP);

864

info->m_constraintError[srow] += mot_fact * limot->m_targetVelocity;

865

info->m_lowerLimit[srow] = -limot->m_maxMotorForce;

866

info->m_upperLimit[srow] = limot->m_maxMotorForce;

867

}

868

}

869

if(limit)

870

{

871

btScalar k = info->fps * limot->m_stopERP;

872

if(!rotational)

873

{

874

info->m_constraintError[srow] += k * limot->m_currentLimitError;

875

}

876

else

877

{

878

info->m_constraintError[srow] += -k * limot->m_currentLimitError;

879

}

880

info->cfm[srow] = limot->m_stopCFM;

881

if (limot->m_loLimit == limot->m_hiLimit)

882

{ // limited low and high simultaneously

883

info->m_lowerLimit[srow] = -SIMD_INFINITY;

884

info->m_upperLimit[srow] = SIMD_INFINITY;

885

}

886

else

887

{

888

if (limit == 1)

889

{

890

info->m_lowerLimit[srow] = 0;

891

info->m_upperLimit[srow] = SIMD_INFINITY;

892

}

893

else

894

{

895

info->m_lowerLimit[srow] = -SIMD_INFINITY;

896

info->m_upperLimit[srow] = 0;

897

}

898

// deal with bounce

899

if (limot->m_bounce > 0)

900

{

901

// calculate joint velocity

902

btScalar vel;

903

if (rotational)

904

{

905

vel = angVelA.dot(ax1);

906

//make sure that if no body -> angVelB == zero vec

907

// if (body1)

908

vel -= angVelB.dot(ax1);

909

}

910

else

911

{

912

vel = linVelA.dot(ax1);

913

//make sure that if no body -> angVelB == zero vec

914

// if (body1)

915

vel -= linVelB.dot(ax1);

916

}

917

// only apply bounce if the velocity is incoming, and if the

918

// resulting c[] exceeds what we already have.

919

if (limit == 1)

920

{

921

if (vel < 0)

922

{

923

btScalar newc = -limot->m_bounce* vel;

924

if (newc > info->m_constraintError[srow])

925

info->m_constraintError[srow] = newc;

926

}

927

}

928

else

929

{

930

if (vel > 0)

931

{

932

btScalar newc = -limot->m_bounce * vel;

933

if (newc < info->m_constraintError[srow])

934

info->m_constraintError[srow] = newc;

935

}

936

}

937

}

938

}

939

}

940

return 1;

941

}

942

else return 0;

943

}

944

945

946

947

948

949

950

///override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0..5).

951

///If no axis is provided, it uses the default axis for this constraint.

952

void btGeneric6DofConstraint::setParam(int num, btScalar value, int axis)

953

{

954

if((axis >= 0) && (axis < 3))

955

{

956

switch(num)

957

{

958

case BT_CONSTRAINT_STOP_ERP :

959

m_linearLimits.m_stopERP[axis] = value;

960

m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);

961

break;

962

case BT_CONSTRAINT_STOP_CFM :

963

m_linearLimits.m_stopCFM[axis] = value;

964

m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);

965

break;

966

case BT_CONSTRAINT_CFM :

967

m_linearLimits.m_normalCFM[axis] = value;

968

m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);

969

break;

970

default :

971

btAssertConstrParams(0);

972

}

973

}

974

else if((axis >=3) && (axis < 6))

975

{

976

switch(num)

977

{

978

case BT_CONSTRAINT_STOP_ERP :

979

m_angularLimits[axis - 3].m_stopERP = value;

980

m_flags |= BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);

981

break;

982

case BT_CONSTRAINT_STOP_CFM :

983

m_angularLimits[axis - 3].m_stopCFM = value;

984

m_flags |= BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);

985

break;

986

case BT_CONSTRAINT_CFM :

987

m_angularLimits[axis - 3].m_normalCFM = value;

988

m_flags |= BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT);

989

break;

990

default :

991

btAssertConstrParams(0);

992

}

993

}

994

else

995

{

996

btAssertConstrParams(0);

997

}

998

}

999

1000

///return the local value of parameter

1001

btScalar btGeneric6DofConstraint::getParam(int num, int axis) const

1002

{

1003

btScalar retVal = 0;

1004

if((axis >= 0) && (axis < 3))

1005

{

1006

switch(num)

1007

{

1008

case BT_CONSTRAINT_STOP_ERP :

1009

btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));

1010

retVal = m_linearLimits.m_stopERP[axis];

1011

break;

1012

case BT_CONSTRAINT_STOP_CFM :

1013

btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));

1014

retVal = m_linearLimits.m_stopCFM[axis];

1015

break;

1016

case BT_CONSTRAINT_CFM :

1017

btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));

1018

retVal = m_linearLimits.m_normalCFM[axis];

1019

break;

1020

default :

1021

btAssertConstrParams(0);

1022

}

1023

}

1024

else if((axis >=3) && (axis < 6))

1025

{

1026

switch(num)

1027

{

1028

case BT_CONSTRAINT_STOP_ERP :

1029

btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_ERP_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));

1030

retVal = m_angularLimits[axis - 3].m_stopERP;

1031

break;

1032

case BT_CONSTRAINT_STOP_CFM :

1033

btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_STOP << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));

1034

retVal = m_angularLimits[axis - 3].m_stopCFM;

1035

break;

1036

case BT_CONSTRAINT_CFM :

1037

btAssertConstrParams(m_flags & (BT_6DOF_FLAGS_CFM_NORM << (axis * BT_6DOF_FLAGS_AXIS_SHIFT)));

1038

retVal = m_angularLimits[axis - 3].m_normalCFM;

1039

break;

1040

default :

1041

btAssertConstrParams(0);

1042

}

1043

}

1044

else

1045

{

1046

btAssertConstrParams(0);

1047

}

1048

return retVal;

1049

}

1050

1051

1052

1053

void btGeneric6DofConstraint::setAxis(const btVector3& axis1,const btVector3& axis2)

1054

{

1055

btVector3 zAxis = axis1.normalized();

1056

btVector3 yAxis = axis2.normalized();

1057

btVector3 xAxis = yAxis.cross(zAxis); // we want right coordinate system

1058

1059

btTransform frameInW;

1060

frameInW.setIdentity();

1061

frameInW.getBasis().setValue( xAxis[0], yAxis[0], zAxis[0],

1062

xAxis[1], yAxis[1], zAxis[1],

1063

xAxis[2], yAxis[2], zAxis[2]);

1064

1065

// now get constraint frame in local coordinate systems

1066

m_frameInA = m_rbA.getCenterOfMassTransform().inverse() * frameInW;

1067

m_frameInB = m_rbB.getCenterOfMassTransform().inverse() * frameInW;

1068

1069

calculateTransforms();

1070

}

b'\\ No newline at end of file'

Older »