~siretart/ubuntu/utopic/blender/libav10

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.

#ifndef BT_MULTIBODY_H

#define BT_MULTIBODY_H

#include "LinearMath/btScalar.h"

#include "LinearMath/btVector3.h"

#include "LinearMath/btQuaternion.h"

#include "LinearMath/btMatrix3x3.h"

#include "LinearMath/btAlignedObjectArray.h"

#include "btMultiBodyLink.h"

class btMultiBodyLinkCollider;

class btMultiBody

{

public:

BT_DECLARE_ALIGNED_ALLOCATOR();

// initialization

btMultiBody(int n_links, // NOT including the base

btScalar mass, // mass of base

const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal

bool fixed_base_, // whether the base is fixed (true) or can move (false)

bool can_sleep_);

~btMultiBody();

void setupPrismatic(int i, // 0 to num_links-1

btScalar mass,

const btVector3 &inertia, // in my frame; assumed diagonal

int parent,

const btQuaternion &rot_parent_to_this, // rotate points in parent frame to my frame.

const btVector3 &joint_axis, // in my frame

const btVector3 &r_vector_when_q_zero, // vector from parent COM to my COM, in my frame, when q = 0.

bool disableParentCollision=false

);

void setupRevolute(int i, // 0 to num_links-1

btScalar mass,

const btVector3 &inertia,

int parent,

const btQuaternion &zero_rot_parent_to_this, // rotate points in parent frame to this frame, when q = 0

const btVector3 &joint_axis, // in my frame

const btVector3 &parent_axis_position, // vector from parent COM to joint axis, in PARENT frame

const btVector3 &my_axis_position, // vector from joint axis to my COM, in MY frame

bool disableParentCollision=false);

const btMultibodyLink& getLink(int index) const

{

return links[index];

}

btMultibodyLink& getLink(int index)

{

return links[index];

}

void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base

{

m_baseCollider = collider;

}

const btMultiBodyLinkCollider* getBaseCollider() const

{

return m_baseCollider;

}

btMultiBodyLinkCollider* getBaseCollider()

{

return m_baseCollider;

}

100

101

// get parent

102

// input: link num from 0 to num_links-1

103

// output: link num from 0 to num_links-1, OR -1 to mean the base.

104

105

int getParent(int link_num) const;

106

107

108

109

// get number of links, masses, moments of inertia

110

111

112

int getNumLinks() const { return links.size(); }

113

btScalar getBaseMass() const { return base_mass; }

114

const btVector3 & getBaseInertia() const { return base_inertia; }

115

btScalar getLinkMass(int i) const;

116

const btVector3 & getLinkInertia(int i) const;

117

118

119

120

// change mass (incomplete: can only change base mass and inertia at present)

121

122

123

void setBaseMass(btScalar mass) { base_mass = mass; }

124

void setBaseInertia(const btVector3 &inertia) { base_inertia = inertia; }

125

126

127

128

// get/set pos/vel/rot/omega for the base link

129

130

131

const btVector3 & getBasePos() const { return base_pos; } // in world frame

132

const btVector3 getBaseVel() const

133

{

134

return btVector3(m_real_buf[3],m_real_buf[4],m_real_buf[5]);

135

} // in world frame

136

const btQuaternion & getWorldToBaseRot() const

137

{

138

return base_quat;

139

} // rotates world vectors into base frame

140

btVector3 getBaseOmega() const { return btVector3(m_real_buf[0],m_real_buf[1],m_real_buf[2]); } // in world frame

141

142

void setBasePos(const btVector3 &pos)

143

{

144

base_pos = pos;

145

}

146

void setBaseVel(const btVector3 &vel)

147

{

148

149

m_real_buf[3]=vel[0]; m_real_buf[4]=vel[1]; m_real_buf[5]=vel[2];

150

}

151

void setWorldToBaseRot(const btQuaternion &rot)

152

{

153

base_quat = rot;

154

}

155

void setBaseOmega(const btVector3 &omega)

156

{

157

m_real_buf[0]=omega[0];

158

m_real_buf[1]=omega[1];

159

m_real_buf[2]=omega[2];

160

}

161

162

163

164

// get/set pos/vel for child links (i = 0 to num_links-1)

165

166

167

btScalar getJointPos(int i) const;

168

btScalar getJointVel(int i) const;

169

170

void setJointPos(int i, btScalar q);

171

void setJointVel(int i, btScalar qdot);

172

173

174

// direct access to velocities as a vector of 6 + num_links elements.

175

// (omega first, then v, then joint velocities.)

176

177

const btScalar * getVelocityVector() const

178

{

179

return &m_real_buf[0];

180

}

181

/* btScalar * getVelocityVector()

182

{

183

return &real_buf[0];

184

}

185

186

187

188

// get the frames of reference (positions and orientations) of the child links

189

// (i = 0 to num_links-1)

190

191

192

const btVector3 & getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords

193

const btQuaternion & getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i.

194

195

196

197

// transform vectors in local frame of link i to world frame (or vice versa)

198

199

btVector3 localPosToWorld(int i, const btVector3 &vec) const;

200

btVector3 localDirToWorld(int i, const btVector3 &vec) const;

201

btVector3 worldPosToLocal(int i, const btVector3 &vec) const;

202

btVector3 worldDirToLocal(int i, const btVector3 &vec) const;

203

204

205

206

// calculate kinetic energy and angular momentum

207

// useful for debugging.

208

209

210

btScalar getKineticEnergy() const;

211

btVector3 getAngularMomentum() const;

212

213

214

215

// set external forces and torques. Note all external forces/torques are given in the WORLD frame.

216

217

218

void clearForcesAndTorques();

219

void clearVelocities();

220

221

void addBaseForce(const btVector3 &f)

222

{

223

base_force += f;

224

}

225

void addBaseTorque(const btVector3 &t) { base_torque += t; }

226

void addLinkForce(int i, const btVector3 &f);

227

void addLinkTorque(int i, const btVector3 &t);

228

void addJointTorque(int i, btScalar Q);

229

230

const btVector3 & getBaseForce() const { return base_force; }

231

const btVector3 & getBaseTorque() const { return base_torque; }

232

const btVector3 & getLinkForce(int i) const;

233

const btVector3 & getLinkTorque(int i) const;

234

btScalar getJointTorque(int i) const;

235

236

237

238

// dynamics routines.

239

240

241

// timestep the velocities (given the external forces/torques set using addBaseForce etc).

242

// also sets up caches for calcAccelerationDeltas.

243

244

// Note: the caller must provide three vectors which are used as

245

// temporary scratch space. The idea here is to reduce dynamic

246

// memory allocation: the same scratch vectors can be re-used

247

// again and again for different Multibodies, instead of each

248

// btMultiBody allocating (and then deallocating) their own

249

// individual scratch buffers. This gives a considerable speed

250

// improvement, at least on Windows (where dynamic memory

251

// allocation appears to be fairly slow).

252

253

void stepVelocities(btScalar dt,

254

btAlignedObjectArray<btScalar> &scratch_r,

255

btAlignedObjectArray<btVector3> &scratch_v,

256

btAlignedObjectArray<btMatrix3x3> &scratch_m);

257

258

// calcAccelerationDeltas

259

// input: force vector (in same format as jacobian, i.e.:

260

// 3 torque values, 3 force values, num_links joint torque values)

261

// output: 3 omegadot values, 3 vdot values, num_links q_double_dot values

262

// (existing contents of output array are replaced)

263

// stepVelocities must have been called first.

264

void calcAccelerationDeltas(const btScalar *force, btScalar *output,

265

btAlignedObjectArray<btScalar> &scratch_r,

266

btAlignedObjectArray<btVector3> &scratch_v) const;

267

268

// apply a delta-vee directly. used in sequential impulses code.

269

void applyDeltaVee(const btScalar * delta_vee)

270

{

271

272

for (int i = 0; i < 6 + getNumLinks(); ++i)

273

{

274

m_real_buf[i] += delta_vee[i];

275

}

276

277

}

278

void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier)

279

{

280

btScalar sum = 0;

281

for (int i = 0; i < 6 + getNumLinks(); ++i)

282

{

283

sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;

284

}

285

btScalar l = btSqrt(sum);

286

287

static btScalar maxl = -1e30f;

288

if (l>maxl)

289

{

290

maxl=l;

291

// printf("maxl=%f\n",maxl);

292

}

293

294

if (l>m_maxAppliedImpulse)

295

{

296

// printf("exceeds 100: l=%f\n",maxl);

297

multiplier *= m_maxAppliedImpulse/l;

298

}

299

300

for (int i = 0; i < 6 + getNumLinks(); ++i)

301

{

302

sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;

303

m_real_buf[i] += delta_vee[i] * multiplier;

304

}

305

}

306

307

// timestep the positions (given current velocities).

308

void stepPositions(btScalar dt);

309

310

311

312

// contacts

313

314

315

// This routine fills out a contact constraint jacobian for this body.

316

// the 'normal' supplied must be -n for body1 or +n for body2 of the contact.

317

// 'normal' & 'contact_point' are both given in world coordinates.

318

void fillContactJacobian(int link,

319

const btVector3 &contact_point,

320

const btVector3 &normal,

321

btScalar *jac,

322

btAlignedObjectArray<btScalar> &scratch_r,

323

btAlignedObjectArray<btVector3> &scratch_v,

324

btAlignedObjectArray<btMatrix3x3> &scratch_m) const;

325

326

327

328

// sleeping

329

330

void setCanSleep(bool canSleep)

331

{

332

can_sleep = canSleep;

333

}

334

335

bool isAwake() const { return awake; }

336

void wakeUp();

337

void goToSleep();

338

void checkMotionAndSleepIfRequired(btScalar timestep);

339

340

bool hasFixedBase() const

341

{

342

return fixed_base;

343

}

344

345

int getCompanionId() const

346

{

347

return m_companionId;

348

}

349

void setCompanionId(int id)

350

{

351

//printf("for %p setCompanionId(%d)\n",this, id);

352

m_companionId = id;

353

}

354

355

void setNumLinks(int numLinks)//careful: when changing the number of links, make sure to re-initialize or update existing links

356

{

357

links.resize(numLinks);

358

}

359

360

btScalar getLinearDamping() const

361

{

362

return m_linearDamping;

363

}

364

void setLinearDamping( btScalar damp)

365

{

366

m_linearDamping = damp;

367

}

368

btScalar getAngularDamping() const

369

{

370

return m_angularDamping;

371

}

372

373

bool getUseGyroTerm() const

374

{

375

return m_useGyroTerm;

376

}

377

void setUseGyroTerm(bool useGyro)

378

{

379

m_useGyroTerm = useGyro;

380

}

381

btScalar getMaxAppliedImpulse() const

382

{

383

return m_maxAppliedImpulse;

384

}

385

void setMaxAppliedImpulse(btScalar maxImp)

386

{

387

m_maxAppliedImpulse = maxImp;

388

}

389

390

void setHasSelfCollision(bool hasSelfCollision)

391

{

392

m_hasSelfCollision = hasSelfCollision;

393

}

394

bool hasSelfCollision() const

395

{

396

return m_hasSelfCollision;

397

}

398

399

private:

400

btMultiBody(const btMultiBody &); // not implemented

401

void operator=(const btMultiBody &); // not implemented

402

403

void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;

404

405

void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const;

406

407

408

private:

409

410

btMultiBodyLinkCollider* m_baseCollider;//can be NULL

411

412

btVector3 base_pos; // position of COM of base (world frame)

413

btQuaternion base_quat; // rotates world points into base frame

414

415

btScalar base_mass; // mass of the base

416

btVector3 base_inertia; // inertia of the base (in local frame; diagonal)

417

418

btVector3 base_force; // external force applied to base. World frame.

419

btVector3 base_torque; // external torque applied to base. World frame.

420

421

btAlignedObjectArray<btMultibodyLink> links; // array of links, excluding the base. index from 0 to num_links-1.

422

btAlignedObjectArray<btMultiBodyLinkCollider*> m_colliders;

423

424

425

// real_buf:

426

// offset size array

427

// 0 6 + num_links v (base_omega; base_vel; joint_vels)

428

// 6+num_links num_links D

429

430

// vector_buf:

431

// offset size array

432

// 0 num_links h_top

433

// num_links num_links h_bottom

434

435

// matrix_buf:

436

// offset size array

437

// 0 num_links+1 rot_from_parent

438

439

440

btAlignedObjectArray<btScalar> m_real_buf;

441

btAlignedObjectArray<btVector3> vector_buf;

442

btAlignedObjectArray<btMatrix3x3> matrix_buf;

443

444

//std::auto_ptr<Eigen::LU<Eigen::Matrix<btScalar, 6, 6> > > cached_imatrix_lu;

445

446

btMatrix3x3 cached_inertia_top_left;

447

btMatrix3x3 cached_inertia_top_right;

448

btMatrix3x3 cached_inertia_lower_left;

449

btMatrix3x3 cached_inertia_lower_right;

450

451

bool fixed_base;

452

453

// Sleep parameters.

454

bool awake;

455

bool can_sleep;

456

btScalar sleep_timer;

457

458

int m_companionId;

459

btScalar m_linearDamping;

460

btScalar m_angularDamping;

461

bool m_useGyroTerm;

462

btScalar m_maxAppliedImpulse;

463

bool m_hasSelfCollision;

464

};

465

466

#endif

Older »