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

///btSoftBodyHelpers.cpp by Nathanael Presson

#include "btSoftBodyInternals.h"

#include <stdio.h>

#include <string.h>

#include "btSoftBodyHelpers.h"

#include "LinearMath/btConvexHull.h"

#include "LinearMath/btConvexHullComputer.h"

static void drawVertex( btIDebugDraw* idraw,

const btVector3& x,btScalar s,const btVector3& c)

{

idraw->drawLine(x-btVector3(s,0,0),x+btVector3(s,0,0),c);

idraw->drawLine(x-btVector3(0,s,0),x+btVector3(0,s,0),c);

idraw->drawLine(x-btVector3(0,0,s),x+btVector3(0,0,s),c);

}

static void drawBox( btIDebugDraw* idraw,

const btVector3& mins,

const btVector3& maxs,

const btVector3& color)

{

const btVector3 c[]={ btVector3(mins.x(),mins.y(),mins.z()),

btVector3(maxs.x(),mins.y(),mins.z()),

btVector3(maxs.x(),maxs.y(),mins.z()),

btVector3(mins.x(),maxs.y(),mins.z()),

btVector3(mins.x(),mins.y(),maxs.z()),

btVector3(maxs.x(),mins.y(),maxs.z()),

btVector3(maxs.x(),maxs.y(),maxs.z()),

btVector3(mins.x(),maxs.y(),maxs.z())};

idraw->drawLine(c[0],c[1],color);idraw->drawLine(c[1],c[2],color);

idraw->drawLine(c[2],c[3],color);idraw->drawLine(c[3],c[0],color);

idraw->drawLine(c[4],c[5],color);idraw->drawLine(c[5],c[6],color);

idraw->drawLine(c[6],c[7],color);idraw->drawLine(c[7],c[4],color);

idraw->drawLine(c[0],c[4],color);idraw->drawLine(c[1],c[5],color);

idraw->drawLine(c[2],c[6],color);idraw->drawLine(c[3],c[7],color);

}

static void drawTree( btIDebugDraw* idraw,

const btDbvtNode* node,

int depth,

const btVector3& ncolor,

const btVector3& lcolor,

int mindepth,

int maxdepth)

{

if(node)

{

if(node->isinternal()&&((depth<maxdepth)||(maxdepth<0)))

{

drawTree(idraw,node->childs[0],depth+1,ncolor,lcolor,mindepth,maxdepth);

drawTree(idraw,node->childs[1],depth+1,ncolor,lcolor,mindepth,maxdepth);

}

if(depth>=mindepth)

{

const btScalar scl=(btScalar)(node->isinternal()?1:1);

const btVector3 mi=node->volume.Center()-node->volume.Extents()*scl;

const btVector3 mx=node->volume.Center()+node->volume.Extents()*scl;

drawBox(idraw,mi,mx,node->isleaf()?lcolor:ncolor);

}

template <typename T>

static inline T sum(const btAlignedObjectArray<T>& items)

{

T v;

if(items.size())

{

v=items[0];

for(int i=1,ni=items.size();i<ni;++i)

{

v+=items[i];

}

return(v);

}

template <typename T,typename Q>

100

static inline void add(btAlignedObjectArray<T>& items,const Q& value)

101

{

102

for(int i=0,ni=items.size();i<ni;++i)

103

{

104

items[i]+=value;

105

}

106

}

107

108

109

template <typename T,typename Q>

110

static inline void mul(btAlignedObjectArray<T>& items,const Q& value)

111

{

112

for(int i=0,ni=items.size();i<ni;++i)

113

{

114

items[i]*=value;

115

}

116

}

117

118

119

template <typename T>

120

static inline T average(const btAlignedObjectArray<T>& items)

121

{

122

const btScalar n=(btScalar)(items.size()>0?items.size():1);

123

return(sum(items)/n);

124

}

125

126

127

static inline btScalar tetravolume(const btVector3& x0,

128

const btVector3& x1,

129

const btVector3& x2,

130

const btVector3& x3)

131

{

132

const btVector3 a=x1-x0;

133

const btVector3 b=x2-x0;

134

const btVector3 c=x3-x0;

135

return(btDot(a,btCross(b,c)));

136

}

137

138

139

#if 0

140

static btVector3 stresscolor(btScalar stress)

141

{

142

static const btVector3 spectrum[]= { btVector3(1,0,1),

143

btVector3(0,0,1),

144

btVector3(0,1,1),

145

btVector3(0,1,0),

146

btVector3(1,1,0),

147

btVector3(1,0,0),

148

btVector3(1,0,0)};

149

static const int ncolors=sizeof(spectrum)/sizeof(spectrum[0])-1;

150

static const btScalar one=1;

151

stress=btMax<btScalar>(0,btMin<btScalar>(1,stress))*ncolors;

152

const int sel=(int)stress;

153

const btScalar frc=stress-sel;

154

return(spectrum[sel]+(spectrum[sel+1]-spectrum[sel])*frc);

155

}

156

#endif

157

158

159

void btSoftBodyHelpers::Draw( btSoftBody* psb,

160

btIDebugDraw* idraw,

161

int drawflags)

162

{

163

const btScalar scl=(btScalar)0.1;

164

const btScalar nscl=scl*5;

165

const btVector3 lcolor=btVector3(0,0,0);

166

const btVector3 ncolor=btVector3(1,1,1);

167

const btVector3 ccolor=btVector3(1,0,0);

168

int i,j,nj;

169

170

/* Clusters */

171

if(0!=(drawflags&fDrawFlags::Clusters))

172

{

173

srand(1806);

174

for(i=0;i<psb->m_clusters.size();++i)

175

{

176

if(psb->m_clusters[i]->m_collide)

177

{

178

btVector3 color( rand()/(btScalar)RAND_MAX,

179

rand()/(btScalar)RAND_MAX,

180

rand()/(btScalar)RAND_MAX);

181

color=color.normalized()*0.75;

182

btAlignedObjectArray<btVector3> vertices;

183

vertices.resize(psb->m_clusters[i]->m_nodes.size());

184

for(j=0,nj=vertices.size();j<nj;++j)

185

{

186

vertices[j]=psb->m_clusters[i]->m_nodes[j]->m_x;

187

}

188

#define USE_NEW_CONVEX_HULL_COMPUTER

189

#ifdef USE_NEW_CONVEX_HULL_COMPUTER

190

btConvexHullComputer computer;

191

int stride = sizeof(btVector3);

192

int count = vertices.size();

193

btScalar shrink=0.f;

194

btScalar shrinkClamp=0.f;

195

computer.compute(&vertices[0].getX(),stride,count,shrink,shrinkClamp);

196

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

197

{

198

199

int face = computer.faces[i];

200

//printf("face=%d\n",face);

201

const btConvexHullComputer::Edge* firstEdge = &computer.edges[face];

202

const btConvexHullComputer::Edge* edge = firstEdge->getNextEdgeOfFace();

203

204

int v0 = firstEdge->getSourceVertex();

205

int v1 = firstEdge->getTargetVertex();

206

while (edge!=firstEdge)

207

{

208

int v2 = edge->getTargetVertex();

209

idraw->drawTriangle(computer.vertices[v0],computer.vertices[v1],computer.vertices[v2],color,1);

210

edge = edge->getNextEdgeOfFace();

211

v0=v1;

212

v1=v2;

213

};

214

}

215

#else

216

217

HullDesc hdsc(QF_TRIANGLES,vertices.size(),&vertices[0]);

218

HullResult hres;

219

HullLibrary hlib;

220

hdsc.mMaxVertices=vertices.size();

221

hlib.CreateConvexHull(hdsc,hres);

222

const btVector3 center=average(hres.m_OutputVertices);

223

add(hres.m_OutputVertices,-center);

224

mul(hres.m_OutputVertices,(btScalar)1);

225

add(hres.m_OutputVertices,center);

226

for(j=0;j<(int)hres.mNumFaces;++j)

227

{

228

const int idx[]={hres.m_Indices[j*3+0],hres.m_Indices[j*3+1],hres.m_Indices[j*3+2]};

229

idraw->drawTriangle(hres.m_OutputVertices[idx[0]],

230

hres.m_OutputVertices[idx[1]],

231

hres.m_OutputVertices[idx[2]],

232

color,1);

233

}

234

hlib.ReleaseResult(hres);

235

#endif

236

237

}

238

/* Velocities */

239

#if 0

240

for(int j=0;j<psb->m_clusters[i].m_nodes.size();++j)

241

{

242

const btSoftBody::Cluster& c=psb->m_clusters[i];

243

const btVector3 r=c.m_nodes[j]->m_x-c.m_com;

244

const btVector3 v=c.m_lv+btCross(c.m_av,r);

245

idraw->drawLine(c.m_nodes[j]->m_x,c.m_nodes[j]->m_x+v,btVector3(1,0,0));

246

}

247

#endif

248

/* Frame */

249

// btSoftBody::Cluster& c=*psb->m_clusters[i];

250

// idraw->drawLine(c.m_com,c.m_framexform*btVector3(10,0,0),btVector3(1,0,0));

251

// idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,10,0),btVector3(0,1,0));

252

// idraw->drawLine(c.m_com,c.m_framexform*btVector3(0,0,10),btVector3(0,0,1));

253

}

254

}

255

else

256

{

257

/* Nodes */

258

if(0!=(drawflags&fDrawFlags::Nodes))

259

{

260

for(i=0;i<psb->m_nodes.size();++i)

261

{

262

const btSoftBody::Node& n=psb->m_nodes[i];

263

if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;

264

idraw->drawLine(n.m_x-btVector3(scl,0,0),n.m_x+btVector3(scl,0,0),btVector3(1,0,0));

265

idraw->drawLine(n.m_x-btVector3(0,scl,0),n.m_x+btVector3(0,scl,0),btVector3(0,1,0));

266

idraw->drawLine(n.m_x-btVector3(0,0,scl),n.m_x+btVector3(0,0,scl),btVector3(0,0,1));

267

}

268

}

269

/* Links */

270

if(0!=(drawflags&fDrawFlags::Links))

271

{

272

for(i=0;i<psb->m_links.size();++i)

273

{

274

const btSoftBody::Link& l=psb->m_links[i];

275

if(0==(l.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;

276

idraw->drawLine(l.m_n[0]->m_x,l.m_n[1]->m_x,lcolor);

277

}

278

}

279

/* Normals */

280

if(0!=(drawflags&fDrawFlags::Normals))

281

{

282

for(i=0;i<psb->m_nodes.size();++i)

283

{

284

const btSoftBody::Node& n=psb->m_nodes[i];

285

if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;

286

const btVector3 d=n.m_n*nscl;

287

idraw->drawLine(n.m_x,n.m_x+d,ncolor);

288

idraw->drawLine(n.m_x,n.m_x-d,ncolor*0.5);

289

}

290

}

291

/* Contacts */

292

if(0!=(drawflags&fDrawFlags::Contacts))

293

{

294

static const btVector3 axis[]={btVector3(1,0,0),

295

btVector3(0,1,0),

296

btVector3(0,0,1)};

297

for(i=0;i<psb->m_rcontacts.size();++i)

298

{

299

const btSoftBody::RContact& c=psb->m_rcontacts[i];

300

const btVector3 o= c.m_node->m_x-c.m_cti.m_normal*

301

(btDot(c.m_node->m_x,c.m_cti.m_normal)+c.m_cti.m_offset);

302

const btVector3 x=btCross(c.m_cti.m_normal,axis[c.m_cti.m_normal.minAxis()]).normalized();

303

const btVector3 y=btCross(x,c.m_cti.m_normal).normalized();

304

idraw->drawLine(o-x*nscl,o+x*nscl,ccolor);

305

idraw->drawLine(o-y*nscl,o+y*nscl,ccolor);

306

idraw->drawLine(o,o+c.m_cti.m_normal*nscl*3,btVector3(1,1,0));

307

}

308

}

309

/* Faces */

310

if(0!=(drawflags&fDrawFlags::Faces))

311

{

312

const btScalar scl=(btScalar)0.8;

313

const btScalar alp=(btScalar)1;

314

const btVector3 col(0,(btScalar)0.7,0);

315

for(i=0;i<psb->m_faces.size();++i)

316

{

317

const btSoftBody::Face& f=psb->m_faces[i];

318

if(0==(f.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;

319

const btVector3 x[]={f.m_n[0]->m_x,f.m_n[1]->m_x,f.m_n[2]->m_x};

320

const btVector3 c=(x[0]+x[1]+x[2])/3;

321

idraw->drawTriangle((x[0]-c)*scl+c,

322

(x[1]-c)*scl+c,

323

(x[2]-c)*scl+c,

324

col,alp);

325

}

326

}

327

/* Tetras */

328

if(0!=(drawflags&fDrawFlags::Tetras))

329

{

330

const btScalar scl=(btScalar)0.8;

331

const btScalar alp=(btScalar)1;

332

const btVector3 col((btScalar)0.7,(btScalar)0.7,(btScalar)0.7);

333

for(int i=0;i<psb->m_tetras.size();++i)

334

{

335

const btSoftBody::Tetra& t=psb->m_tetras[i];

336

if(0==(t.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;

337

const btVector3 x[]={t.m_n[0]->m_x,t.m_n[1]->m_x,t.m_n[2]->m_x,t.m_n[3]->m_x};

338

const btVector3 c=(x[0]+x[1]+x[2]+x[3])/4;

339

idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[2]-c)*scl+c,col,alp);

340

idraw->drawTriangle((x[0]-c)*scl+c,(x[1]-c)*scl+c,(x[3]-c)*scl+c,col,alp);

341

idraw->drawTriangle((x[1]-c)*scl+c,(x[2]-c)*scl+c,(x[3]-c)*scl+c,col,alp);

342

idraw->drawTriangle((x[2]-c)*scl+c,(x[0]-c)*scl+c,(x[3]-c)*scl+c,col,alp);

343

}

344

}

345

}

346

/* Anchors */

347

if(0!=(drawflags&fDrawFlags::Anchors))

348

{

349

for(i=0;i<psb->m_anchors.size();++i)

350

{

351

const btSoftBody::Anchor& a=psb->m_anchors[i];

352

const btVector3 q=a.m_body->getWorldTransform()*a.m_local;

353

drawVertex(idraw,a.m_node->m_x,0.25,btVector3(1,0,0));

354

drawVertex(idraw,q,0.25,btVector3(0,1,0));

355

idraw->drawLine(a.m_node->m_x,q,btVector3(1,1,1));

356

}

357

for(i=0;i<psb->m_nodes.size();++i)

358

{

359

const btSoftBody::Node& n=psb->m_nodes[i];

360

if(0==(n.m_material->m_flags&btSoftBody::fMaterial::DebugDraw)) continue;

361

if(n.m_im<=0)

362

{

363

drawVertex(idraw,n.m_x,0.25,btVector3(1,0,0));

364

}

365

}

366

}

367

368

369

/* Notes */

370

if(0!=(drawflags&fDrawFlags::Notes))

371

{

372

for(i=0;i<psb->m_notes.size();++i)

373

{

374

const btSoftBody::Note& n=psb->m_notes[i];

375

btVector3 p=n.m_offset;

376

for(int j=0;j<n.m_rank;++j)

377

{

378

p+=n.m_nodes[j]->m_x*n.m_coords[j];

379

}

380

idraw->draw3dText(p,n.m_text);

381

}

382

}

383

/* Node tree */

384

if(0!=(drawflags&fDrawFlags::NodeTree)) DrawNodeTree(psb,idraw);

385

/* Face tree */

386

if(0!=(drawflags&fDrawFlags::FaceTree)) DrawFaceTree(psb,idraw);

387

/* Cluster tree */

388

if(0!=(drawflags&fDrawFlags::ClusterTree)) DrawClusterTree(psb,idraw);

389

/* Joints */

390

if(0!=(drawflags&fDrawFlags::Joints))

391

{

392

for(i=0;i<psb->m_joints.size();++i)

393

{

394

const btSoftBody::Joint* pj=psb->m_joints[i];

395

switch(pj->Type())

396

{

397

case btSoftBody::Joint::eType::Linear:

398

{

399

const btSoftBody::LJoint* pjl=(const btSoftBody::LJoint*)pj;

400

const btVector3 a0=pj->m_bodies[0].xform()*pjl->m_refs[0];

401

const btVector3 a1=pj->m_bodies[1].xform()*pjl->m_refs[1];

402

idraw->drawLine(pj->m_bodies[0].xform().getOrigin(),a0,btVector3(1,1,0));

403

idraw->drawLine(pj->m_bodies[1].xform().getOrigin(),a1,btVector3(0,1,1));

404

drawVertex(idraw,a0,0.25,btVector3(1,1,0));

405

drawVertex(idraw,a1,0.25,btVector3(0,1,1));

406

}

407

break;

408

case btSoftBody::Joint::eType::Angular:

409

{

410

//const btSoftBody::AJoint* pja=(const btSoftBody::AJoint*)pj;

411

const btVector3 o0=pj->m_bodies[0].xform().getOrigin();

412

const btVector3 o1=pj->m_bodies[1].xform().getOrigin();

413

const btVector3 a0=pj->m_bodies[0].xform().getBasis()*pj->m_refs[0];

414

const btVector3 a1=pj->m_bodies[1].xform().getBasis()*pj->m_refs[1];

415

idraw->drawLine(o0,o0+a0*10,btVector3(1,1,0));

416

idraw->drawLine(o0,o0+a1*10,btVector3(1,1,0));

417

idraw->drawLine(o1,o1+a0*10,btVector3(0,1,1));

418

idraw->drawLine(o1,o1+a1*10,btVector3(0,1,1));

419

break;

420

}

421

default:

422

{

423

}

424

425

}

426

}

427

}

428

}

429

430

431

void btSoftBodyHelpers::DrawInfos( btSoftBody* psb,

432

btIDebugDraw* idraw,

433

bool masses,

434

bool areas,

435

bool /*stress*/)

436

{

437

for(int i=0;i<psb->m_nodes.size();++i)

438

{

439

const btSoftBody::Node& n=psb->m_nodes[i];

440

char text[2048]={0};

441

char buff[1024];

442

if(masses)

443

{

444

sprintf(buff," M(%.2f)",1/n.m_im);

445

strcat(text,buff);

446

}

447

if(areas)

448

{

449

sprintf(buff," A(%.2f)",n.m_area);

450

strcat(text,buff);

451

}

452

if(text[0]) idraw->draw3dText(n.m_x,text);

453

}

454

}

455

456

457

void btSoftBodyHelpers::DrawNodeTree( btSoftBody* psb,

458

btIDebugDraw* idraw,

459

int mindepth,

460

int maxdepth)

461

{

462

drawTree(idraw,psb->m_ndbvt.m_root,0,btVector3(1,0,1),btVector3(1,1,1),mindepth,maxdepth);

463

}

464

465

466

void btSoftBodyHelpers::DrawFaceTree( btSoftBody* psb,

467

btIDebugDraw* idraw,

468

int mindepth,

469

int maxdepth)

470

{

471

drawTree(idraw,psb->m_fdbvt.m_root,0,btVector3(0,1,0),btVector3(1,0,0),mindepth,maxdepth);

472

}

473

474

475

void btSoftBodyHelpers::DrawClusterTree( btSoftBody* psb,

476

btIDebugDraw* idraw,

477

int mindepth,

478

int maxdepth)

479

{

480

drawTree(idraw,psb->m_cdbvt.m_root,0,btVector3(0,1,1),btVector3(1,0,0),mindepth,maxdepth);

481

}

482

483

484

void btSoftBodyHelpers::DrawFrame( btSoftBody* psb,

485

btIDebugDraw* idraw)

486

{

487

if(psb->m_pose.m_bframe)

488

{

489

static const btScalar ascl=10;

490

static const btScalar nscl=(btScalar)0.1;

491

const btVector3 com=psb->m_pose.m_com;

492

const btMatrix3x3 trs=psb->m_pose.m_rot*psb->m_pose.m_scl;

493

const btVector3 Xaxis=(trs*btVector3(1,0,0)).normalized();

494

const btVector3 Yaxis=(trs*btVector3(0,1,0)).normalized();

495

const btVector3 Zaxis=(trs*btVector3(0,0,1)).normalized();

496

idraw->drawLine(com,com+Xaxis*ascl,btVector3(1,0,0));

497

idraw->drawLine(com,com+Yaxis*ascl,btVector3(0,1,0));

498

idraw->drawLine(com,com+Zaxis*ascl,btVector3(0,0,1));

499

for(int i=0;i<psb->m_pose.m_pos.size();++i)

500

{

501

const btVector3 x=com+trs*psb->m_pose.m_pos[i];

502

drawVertex(idraw,x,nscl,btVector3(1,0,1));

503

}

504

}

505

}

506

507

508

btSoftBody* btSoftBodyHelpers::CreateRope( btSoftBodyWorldInfo& worldInfo, const btVector3& from,

509

const btVector3& to,

510

int res,

511

int fixeds)

512

{

513

/* Create nodes */

514

const int r=res+2;

515

btVector3* x=new btVector3[r];

516

btScalar* m=new btScalar[r];

517

int i;

518

519

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

520

{

521

const btScalar t=i/(btScalar)(r-1);

522

x[i]=lerp(from,to,t);

523

m[i]=1;

524

}

525

btSoftBody* psb= new btSoftBody(&worldInfo,r,x,m);

526

if(fixeds&1) psb->setMass(0,0);

527

if(fixeds&2) psb->setMass(r-1,0);

528

delete[] x;

529

delete[] m;

530

/* Create links */

531

for(i=1;i<r;++i)

532

{

533

psb->appendLink(i-1,i);

534

}

535

/* Finished */

536

return(psb);

537

}

538

539

540

btSoftBody* btSoftBodyHelpers::CreatePatch(btSoftBodyWorldInfo& worldInfo,const btVector3& corner00,

541

const btVector3& corner10,

542

const btVector3& corner01,

543

const btVector3& corner11,

544

int resx,

545

int resy,

546

int fixeds,

547

bool gendiags)

548

{

549

#define IDX(_x_,_y_) ((_y_)*rx+(_x_))

550

/* Create nodes */

551

if((resx<2)||(resy<2)) return(0);

552

const int rx=resx;

553

const int ry=resy;

554

const int tot=rx*ry;

555

btVector3* x=new btVector3[tot];

556

btScalar* m=new btScalar[tot];

557

int iy;

558

559

for(iy=0;iy<ry;++iy)

560

{

561

const btScalar ty=iy/(btScalar)(ry-1);

562

const btVector3 py0=lerp(corner00,corner01,ty);

563

const btVector3 py1=lerp(corner10,corner11,ty);

564

for(int ix=0;ix<rx;++ix)

565

{

566

const btScalar tx=ix/(btScalar)(rx-1);

567

x[IDX(ix,iy)]=lerp(py0,py1,tx);

568

m[IDX(ix,iy)]=1;

569

}

570

}

571

btSoftBody* psb=new btSoftBody(&worldInfo,tot,x,m);

572

if(fixeds&1) psb->setMass(IDX(0,0),0);

573

if(fixeds&2) psb->setMass(IDX(rx-1,0),0);

574

if(fixeds&4) psb->setMass(IDX(0,ry-1),0);

575

if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0);

576

delete[] x;

577

delete[] m;

578

/* Create links and faces */

579

for(iy=0;iy<ry;++iy)

580

{

581

for(int ix=0;ix<rx;++ix)

582

{

583

const int idx=IDX(ix,iy);

584

const bool mdx=(ix+1)<rx;

585

const bool mdy=(iy+1)<ry;

586

if(mdx) psb->appendLink(idx,IDX(ix+1,iy));

587

if(mdy) psb->appendLink(idx,IDX(ix,iy+1));

588

if(mdx&&mdy)

589

{

590

if((ix+iy)&1)

591

{

592

psb->appendFace(IDX(ix,iy),IDX(ix+1,iy),IDX(ix+1,iy+1));

593

psb->appendFace(IDX(ix,iy),IDX(ix+1,iy+1),IDX(ix,iy+1));

594

if(gendiags)

595

{

596

psb->appendLink(IDX(ix,iy),IDX(ix+1,iy+1));

597

}

598

}

599

else

600

{

601

psb->appendFace(IDX(ix,iy+1),IDX(ix,iy),IDX(ix+1,iy));

602

psb->appendFace(IDX(ix,iy+1),IDX(ix+1,iy),IDX(ix+1,iy+1));

603

if(gendiags)

604

{

605

psb->appendLink(IDX(ix+1,iy),IDX(ix,iy+1));

606

}

607

}

608

}

609

}

610

}

611

/* Finished */

612

#undef IDX

613

return(psb);

614

}

615

616

617

btSoftBody* btSoftBodyHelpers::CreatePatchUV(btSoftBodyWorldInfo& worldInfo,

618

const btVector3& corner00,

619

const btVector3& corner10,

620

const btVector3& corner01,

621

const btVector3& corner11,

622

int resx,

623

int resy,

624

int fixeds,

625

bool gendiags,

626

float* tex_coords)

627

{

628

629

630

631

* corners:

632

633

* [0][0] corner00 ------- corner01 [resx][0]

634

* | |

635

* | |

636

* [0][resy] corner10 -------- corner11 [resx][resy]

637

638

639

640

641

642

643

* "fixedgs" map:

644

645

* corner00 --> +1

646

* corner01 --> +2

647

* corner10 --> +4

648

* corner11 --> +8

649

* upper middle --> +16

650

* left middle --> +32

651

* right middle --> +64

652

* lower middle --> +128

653

* center --> +256

654

655

656

* tex_coords size (resx-1)*(resy-1)*12

657

658

659

660

* SINGLE QUAD INTERNALS

661

662

* 1) btSoftBody's nodes and links,

663

* diagonal link is optional ("gendiags")

664

665

666

* node00 ------ node01

667

* | .

668

* | .

669

* | .

670

* | .

671

* | .

672

* node10 node11

673

674

675

676

* 2) Faces:

677

* two triangles,

678

* UV Coordinates (hier example for single quad)

679

680

* (0,1) (0,1) (1,1)

681

* 1 |\ 3 \-----| 2

682

* | \ \ |

683

* | \ \ |

684

* | \ \ |

685

* | \ \ |

686

* 2 |-----\ 3 \| 1

687

* (0,0) (1,0) (1,0)

688

689

690

691

692

693

694

695

696

#define IDX(_x_,_y_) ((_y_)*rx+(_x_))

697

/* Create nodes */

698

if((resx<2)||(resy<2)) return(0);

699

const int rx=resx;

700

const int ry=resy;

701

const int tot=rx*ry;

702

btVector3* x=new btVector3[tot];

703

btScalar* m=new btScalar[tot];

704

705

int iy;

706

707

for(iy=0;iy<ry;++iy)

708

{

709

const btScalar ty=iy/(btScalar)(ry-1);

710

const btVector3 py0=lerp(corner00,corner01,ty);

711

const btVector3 py1=lerp(corner10,corner11,ty);

712

for(int ix=0;ix<rx;++ix)

713

{

714

const btScalar tx=ix/(btScalar)(rx-1);

715

x[IDX(ix,iy)]=lerp(py0,py1,tx);

716

m[IDX(ix,iy)]=1;

717

}

718

}

719

btSoftBody* psb=new btSoftBody(&worldInfo,tot,x,m);

720

if(fixeds&1) psb->setMass(IDX(0,0),0);

721

if(fixeds&2) psb->setMass(IDX(rx-1,0),0);

722

if(fixeds&4) psb->setMass(IDX(0,ry-1),0);

723

if(fixeds&8) psb->setMass(IDX(rx-1,ry-1),0);

724

if(fixeds&16) psb->setMass(IDX((rx-1)/2,0),0);

725

if(fixeds&32) psb->setMass(IDX(0,(ry-1)/2),0);

726

if(fixeds&64) psb->setMass(IDX(rx-1,(ry-1)/2),0);

727

if(fixeds&128) psb->setMass(IDX((rx-1)/2,ry-1),0);

728

if(fixeds&256) psb->setMass(IDX((rx-1)/2,(ry-1)/2),0);

729

delete[] x;

730

delete[] m;

731

732

733

int z = 0;

734

/* Create links and faces */

735

for(iy=0;iy<ry;++iy)

736

{

737

for(int ix=0;ix<rx;++ix)

738

{

739

const bool mdx=(ix+1)<rx;

740

const bool mdy=(iy+1)<ry;

741

742

int node00=IDX(ix,iy);

743

int node01=IDX(ix+1,iy);

744

int node10=IDX(ix,iy+1);

745

int node11=IDX(ix+1,iy+1);

746

747

if(mdx) psb->appendLink(node00,node01);

748

if(mdy) psb->appendLink(node00,node10);

749

if(mdx&&mdy)

750

{

751

psb->appendFace(node00,node10,node11);

752

if (tex_coords) {

753

tex_coords[z+0]=CalculateUV(resx,resy,ix,iy,0);

754

tex_coords[z+1]=CalculateUV(resx,resy,ix,iy,1);

755

tex_coords[z+2]=CalculateUV(resx,resy,ix,iy,0);

756

tex_coords[z+3]=CalculateUV(resx,resy,ix,iy,2);

757

tex_coords[z+4]=CalculateUV(resx,resy,ix,iy,3);

758

tex_coords[z+5]=CalculateUV(resx,resy,ix,iy,2);

759

}

760

psb->appendFace(node11,node01,node00);

761

if (tex_coords) {

762

tex_coords[z+6 ]=CalculateUV(resx,resy,ix,iy,3);

763

tex_coords[z+7 ]=CalculateUV(resx,resy,ix,iy,2);

764

tex_coords[z+8 ]=CalculateUV(resx,resy,ix,iy,3);

765

tex_coords[z+9 ]=CalculateUV(resx,resy,ix,iy,1);

766

tex_coords[z+10]=CalculateUV(resx,resy,ix,iy,0);

767

tex_coords[z+11]=CalculateUV(resx,resy,ix,iy,1);

768

}

769

if (gendiags) psb->appendLink(node00,node11);

770

z += 12;

771

}

772

}

773

}

774

/* Finished */

775

#undef IDX

776

return(psb);

777

}

778

779

float btSoftBodyHelpers::CalculateUV(int resx,int resy,int ix,int iy,int id)

780

{

781

782

783

784

785

* node00 --- node01

786

* | |

787

* node10 --- node11

788

789

790

* ID map:

791

792

* node00 s --> 0

793

* node00 t --> 1

794

795

* node01 s --> 3

796

* node01 t --> 1

797

798

* node10 s --> 0

799

* node10 t --> 2

800

801

* node11 s --> 3

802

* node11 t --> 2

803

804

805

806

807

float tc=0.0f;

808

if (id == 0) {

809

tc = (1.0f/((resx-1))*ix);

810

}

811

else if (id==1) {

812

tc = (1.0f/((resy-1))*(resy-1-iy));

813

}

814

else if (id==2) {

815

tc = (1.0f/((resy-1))*(resy-1-iy-1));

816

}

817

else if (id==3) {

818

tc = (1.0f/((resx-1))*(ix+1));

819

}

820

return tc;

821

}

822

823

btSoftBody* btSoftBodyHelpers::CreateEllipsoid(btSoftBodyWorldInfo& worldInfo,const btVector3& center,

824

const btVector3& radius,

825

int res)

826

{

827

struct Hammersley

828

{

829

static void Generate(btVector3* x,int n)

830

{

831

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

832

{

833

btScalar p=0.5,t=0;

834

for(int j=i;j;p*=0.5,j>>=1) if(j&1) t+=p;

835

btScalar w=2*t-1;

836

btScalar a=(SIMD_PI+2*i*SIMD_PI)/n;

837

btScalar s=btSqrt(1-w*w);

838

*x++=btVector3(s*btCos(a),s*btSin(a),w);

839

}

840

}

841

};

842

btAlignedObjectArray<btVector3> vtx;

843

vtx.resize(3+res);

844

Hammersley::Generate(&vtx[0],vtx.size());

845

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

846

{

847

vtx[i]=vtx[i]*radius+center;

848

}

849

return(CreateFromConvexHull(worldInfo,&vtx[0],vtx.size()));

850

}

851

852

853

854

855

btSoftBody* btSoftBodyHelpers::CreateFromTriMesh(btSoftBodyWorldInfo& worldInfo,const btScalar* vertices,

856

const int* triangles,

857

int ntriangles, bool randomizeConstraints)

858

{

859

int maxidx=0;

860

int i,j,ni;

861

862

for(i=0,ni=ntriangles*3;i<ni;++i)

863

{

864

maxidx=btMax(triangles[i],maxidx);

865

}

866

++maxidx;

867

btAlignedObjectArray<bool> chks;

868

btAlignedObjectArray<btVector3> vtx;

869

chks.resize(maxidx*maxidx,false);

870

vtx.resize(maxidx);

871

for(i=0,j=0,ni=maxidx*3;i<ni;++j,i+=3)

872

{

873

vtx[j]=btVector3(vertices[i],vertices[i+1],vertices[i+2]);

874

}

875

btSoftBody* psb=new btSoftBody(&worldInfo,vtx.size(),&vtx[0],0);

876

for( i=0,ni=ntriangles*3;i<ni;i+=3)

877

{

878

const int idx[]={triangles[i],triangles[i+1],triangles[i+2]};

879

#define IDX(_x_,_y_) ((_y_)*maxidx+(_x_))

880

for(int j=2,k=0;k<3;j=k++)

881

{

882

if(!chks[IDX(idx[j],idx[k])])

883

{

884

chks[IDX(idx[j],idx[k])]=true;

885

chks[IDX(idx[k],idx[j])]=true;

886

psb->appendLink(idx[j],idx[k]);

887

}

888

}

889

#undef IDX

890

psb->appendFace(idx[0],idx[1],idx[2]);

891

}

892

893

if (randomizeConstraints)

894

{

895

psb->randomizeConstraints();

896

}

897

898

return(psb);

899

}

900

901

902

btSoftBody* btSoftBodyHelpers::CreateFromConvexHull(btSoftBodyWorldInfo& worldInfo, const btVector3* vertices,

903

int nvertices, bool randomizeConstraints)

904

{

905

HullDesc hdsc(QF_TRIANGLES,nvertices,vertices);

906

HullResult hres;

907

HullLibrary hlib;/*??*/

908

hdsc.mMaxVertices=nvertices;

909

hlib.CreateConvexHull(hdsc,hres);

910

btSoftBody* psb=new btSoftBody(&worldInfo,(int)hres.mNumOutputVertices,

911

&hres.m_OutputVertices[0],0);

912

for(int i=0;i<(int)hres.mNumFaces;++i)

913

{

914

const int idx[]={ hres.m_Indices[i*3+0],

915

hres.m_Indices[i*3+1],

916

hres.m_Indices[i*3+2]};

917

if(idx[0]<idx[1]) psb->appendLink( idx[0],idx[1]);

918

if(idx[1]<idx[2]) psb->appendLink( idx[1],idx[2]);

919

if(idx[2]<idx[0]) psb->appendLink( idx[2],idx[0]);

920

psb->appendFace(idx[0],idx[1],idx[2]);

921

}

922

hlib.ReleaseResult(hres);

923

if (randomizeConstraints)

924

{

925

psb->randomizeConstraints();

926

}

927

return(psb);

928

}

929

930

931

932

933

static int nextLine(const char* buffer)

934

{

935

int numBytesRead=0;

936

937

while (*buffer != '\n')

938

{

939

buffer++;

940

numBytesRead++;

941

}

942

943

944

if (buffer[0]==0x0a)

945

{

946

buffer++;

947

numBytesRead++;

948

}

949

return numBytesRead;

950

}

951

952

/* Create from TetGen .ele, .face, .node data */

953

btSoftBody* btSoftBodyHelpers::CreateFromTetGenData(btSoftBodyWorldInfo& worldInfo,

954

const char* ele,

955

const char* face,

956

const char* node,

957

bool bfacelinks,

958

bool btetralinks,

959

bool bfacesfromtetras)

960

{

961

btAlignedObjectArray<btVector3> pos;

962

int nnode=0;

963

int ndims=0;

964

int nattrb=0;

965

int hasbounds=0;

966

int result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds);

967

result = sscanf(node,"%d %d %d %d",&nnode,&ndims,&nattrb,&hasbounds);

968

node += nextLine(node);

969

970

pos.resize(nnode);

971

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

972

{

973

int index=0;

974

//int bound=0;

975

float x,y,z;

976

sscanf(node,"%d %f %f %f",&index,&x,&y,&z);

977

978

// sn>>index;

979

// sn>>x;sn>>y;sn>>z;

980

node += nextLine(node);

981

982

//for(int j=0;j<nattrb;++j)

983

// sn>>a;

984

985

//if(hasbounds)

986

// sn>>bound;

987

988

pos[index].setX(btScalar(x));

989

pos[index].setY(btScalar(y));

990

pos[index].setZ(btScalar(z));

991

}

992

btSoftBody* psb=new btSoftBody(&worldInfo,nnode,&pos[0],0);

993

#if 0

994

if(face&&face[0])

995

{

996

int nface=0;

997

sf>>nface;sf>>hasbounds;

998

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

999

{

1000

int index=0;

1001

int bound=0;

1002

int ni[3];

1003

sf>>index;

1004

sf>>ni[0];sf>>ni[1];sf>>ni[2];

1005

sf>>bound;

1006

psb->appendFace(ni[0],ni[1],ni[2]);

1007

if(btetralinks)

1008

{

1009

psb->appendLink(ni[0],ni[1],0,true);

1010

psb->appendLink(ni[1],ni[2],0,true);

1011

psb->appendLink(ni[2],ni[0],0,true);

1012

}

1013

}

1014

}

1015

#endif

1016

1017

if(ele&&ele[0])

1018

{

1019

int ntetra=0;

1020

int ncorner=0;

1021

int neattrb=0;

1022

sscanf(ele,"%d %d %d",&ntetra,&ncorner,&neattrb);

1023

ele += nextLine(ele);

1024

1025

//se>>ntetra;se>>ncorner;se>>neattrb;

1026

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

1027

{

1028

int index=0;

1029

int ni[4];

1030

1031

//se>>index;

1032

//se>>ni[0];se>>ni[1];se>>ni[2];se>>ni[3];

1033

sscanf(ele,"%d %d %d %d %d",&index,&ni[0],&ni[1],&ni[2],&ni[3]);

1034

ele+=nextLine(ele);

1035

//for(int j=0;j<neattrb;++j)

1036

// se>>a;

1037

psb->appendTetra(ni[0],ni[1],ni[2],ni[3]);

1038

if(btetralinks)

1039

{

1040

psb->appendLink(ni[0],ni[1],0,true);

1041

psb->appendLink(ni[1],ni[2],0,true);

1042

psb->appendLink(ni[2],ni[0],0,true);

1043

psb->appendLink(ni[0],ni[3],0,true);

1044

psb->appendLink(ni[1],ni[3],0,true);

1045

psb->appendLink(ni[2],ni[3],0,true);

1046

}

1047

}

1048

}

1049

printf("Nodes: %u\r\n",psb->m_nodes.size());

1050

printf("Links: %u\r\n",psb->m_links.size());

1051

printf("Faces: %u\r\n",psb->m_faces.size());

1052

printf("Tetras: %u\r\n",psb->m_tetras.size());

1053

return(psb);

1054

}

1055

Older »