Demos/SoftDemo/SoftDemo.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
   4
   5 This software is provided 'as-is', without any express or implied warranty.
   6 In no event will the authors be held liable for any damages arising from the use of this software.
   7 Permission is granted to anyone to use this software for any purpose,
   8 including commercial applications, and to alter it and redistribute it freely,
   9 subject to the following restrictions:
  10
  11 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.
  12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
  13 3. This notice may not be removed or altered from any source distribution.
  14 */
  15
  16 ///btSoftBody implementation by Nathanael Presson
  17
  18
  19 #include "btBulletDynamicsCommon.h"
  20 #include "BulletSoftBody/btSoftRigidDynamicsWorld.h"
  21
  22 #include "BulletCollision/CollisionDispatch/btSphereSphereCollisionAlgorithm.h"
  23 #include "BulletCollision/NarrowPhaseCollision/btGjkEpa2.h"
  24 #include "LinearMath/btQuickprof.h"
  25 #include "LinearMath/btIDebugDraw.h"
  26
  27 #include "../GimpactTestDemo/BunnyMesh.h"
  28 #include "../GimpactTestDemo/TorusMesh.h"
  29 #include <stdio.h> //printf debugging
  30 #include "LinearMath/btConvexHull.h"
  31 #include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
  32 #include "BulletSoftBody/btSoftBodyHelpers.h"
  33
  34 #include "SoftDemo.h"
  35 #include "GL_ShapeDrawer.h"
  36 #include "GLDebugFont.h"
  37 #include "GlutStuff.h"
  38
  39 extern float eye[3];
  40 extern int glutScreenWidth;
  41 extern int glutScreenHeight;
  42
  43 static bool sDemoMode = false;
  44
  45 const int maxProxies = 32766;
  46 const int maxOverlap = 65535;
  47
  48 static btVector3*       gGroundVertices=0;
  49 static int*     gGroundIndices=0;
  50 static btBvhTriangleMeshShape* trimeshShape =0;
  51 static btRigidBody* staticBody = 0;
  52 static float waveheight = 5.f;
  53
  54 const float TRIANGLE_SIZE=8.f;
  55 unsigned int            current_demo=7;
  56 #define DEMO_MODE_TIMEOUT 15.f //15 seconds for each demo
  57
  58
  59 #ifdef _DEBUG
  60 const int gNumObjects = 1;
  61 #else
  62 const int gNumObjects = 1;//try this in release mode: 3000. never go above 16384, unless you increate maxNumObjects  value in DemoApplication.cp
  63 #endif
  64
  65 const int maxNumObjects = 32760;
  66
  67 #define CUBE_HALF_EXTENTS 1.5
  68 #define EXTRA_HEIGHT -10.f
  69
  70
  71 #ifdef USE_AMD_OPENCL
  72 #include "btOpenCLUtils.h"
  73 #include "BulletMultiThreaded/GpuSoftBodySolvers/OpenCL/btSoftBodySolver_OpenCL.h"
  74 #include "BulletMultiThreaded/GpuSoftBodySolvers/OpenCL/btSoftBodySolver_OpenCLSIMDAware.h"
  75 #include "BulletMultiThreaded/GpuSoftBodySolvers/OpenCL/btSoftBodySolverVertexBuffer_OpenGL.h"
  76
  77 btOpenCLSoftBodySolver* g_openCLSIMDSolver=0;
  78 btSoftBodySolverOutputCLtoCPU* g_softBodyOutput = 0;
  79
  80 cl_context                      g_cxMainContext;
  81 cl_device_id            g_cdDevice;
  82 cl_command_queue        g_cqCommandQue;
  83
  84 void initCL( void* glCtx, void* glDC )
  85 {
  86         int ciErrNum = 0;
  87
  88 #if defined(CL_PLATFORM_MINI_CL)
  89         cl_device_type deviceType = CL_DEVICE_TYPE_CPU;//or use CL_DEVICE_TYPE_DEBUG to debug MiniCL
  90 #elif defined(CL_PLATFORM_INTEL)
  91         cl_device_type deviceType = CL_DEVICE_TYPE_CPU;
  92 #elif defined(CL_PLATFORM_AMD)
  93         cl_device_type deviceType = CL_DEVICE_TYPE_GPU;
  94 #elif defined(CL_PLATFORM_NVIDIA)
  95         cl_device_type deviceType = CL_DEVICE_TYPE_GPU;
  96 #else
  97 #ifdef __APPLE__
  98         cl_device_type deviceType = CL_DEVICE_TYPE_ALL;//GPU;
  99 #else
 100         cl_device_type deviceType = CL_DEVICE_TYPE_CPU;//CL_DEVICE_TYPE_ALL
 101 #endif//__APPLE__
 102 #endif
 103
 104         g_cxMainContext = btOpenCLUtils::createContextFromType(deviceType, &ciErrNum, glCtx, glDC);
 105         oclCHECKERROR(ciErrNum, CL_SUCCESS);
 106
 107
 108         int numDev = btOpenCLUtils::getNumDevices(g_cxMainContext);
 109         if (!numDev)
 110         {
 111                 btAssert(0);
 112                 exit(0);//this is just a demo, exit now
 113         }
 114
 115         g_cdDevice = btOpenCLUtils::getDevice(g_cxMainContext,0);
 116         oclCHECKERROR(ciErrNum, CL_SUCCESS);
 117
 118         btOpenCLDeviceInfo clInfo;
 119         btOpenCLUtils::getDeviceInfo(g_cdDevice,clInfo);
 120         btOpenCLUtils::printDeviceInfo(g_cdDevice);
 121
 122         // create a command-queue
 123         g_cqCommandQue = clCreateCommandQueue(g_cxMainContext, g_cdDevice, 0, &ciErrNum);
 124         oclCHECKERROR(ciErrNum, CL_SUCCESS);
 125 }
 126
 127 class CachingCLFunctions : public CLFunctions
 128 {
 129 protected:
 130
 131         cl_device_id            m_device;
 132
 133         const char* strip(const char* name, const char* pattern);
 134
 135 public:
 136         CachingCLFunctions(cl_command_queue cqCommandQue, cl_context cxMainContext) :
 137                 CLFunctions(cqCommandQue,cxMainContext)
 138         {
 139                 size_t actualSize;
 140                 cl_int retval = clGetCommandQueueInfo ( cqCommandQue, CL_QUEUE_DEVICE, sizeof(cl_device_id),
 141                         &m_device, &actualSize);
 142         }
 143
 144         /**
 145          * Compile a compute shader kernel from a string and return the appropriate cl_kernel object.
 146          */
 147         virtual cl_kernel compileCLKernelFromString( const char* kernelSource, const char* kernelName, const char* additionalMacros , const char* orgSrcFileNameForCaching)
 148         {
 149                 char srcFileNameForCaching[1024];
 150                 sprintf(srcFileNameForCaching,"%s/%s","../../src/BulletMultiThreaded/GpuSoftBodySolvers/OpenCL",orgSrcFileNameForCaching);
 151
 152                 btAssert(additionalMacros);
 153                 btAssert(srcFileNameForCaching && strlen(srcFileNameForCaching));
 154
 155                 printf("compiling kernelName: %s ",kernelName);
 156                 cl_kernel kernel=0;
 157                 cl_int ciErrNum;
 158
 159
 160                 size_t program_length = strlen(kernelSource);
 161
 162                 cl_program m_cpProgram = btOpenCLUtils::compileCLProgramFromString(m_cxMainContext, m_device, kernelSource,  &ciErrNum, additionalMacros);
 163
 164
 165                 // Create the kernel
 166                 kernel = clCreateKernel(m_cpProgram, kernelName, &ciErrNum);
 167                 if (ciErrNum != CL_SUCCESS)
 168                 {
 169                         const char* msg = "";
 170                         switch(ciErrNum)
 171                         {
 172                         case CL_INVALID_PROGRAM:
 173                                 msg = "Program is not a valid program object.";
 174                                 break;
 175                         case CL_INVALID_PROGRAM_EXECUTABLE:
 176                                 msg = "There is no successfully built executable for program.";
 177                                 break;
 178                         case CL_INVALID_KERNEL_NAME:
 179                                 msg = "kernel_name is not found in program.";
 180                                 break;
 181                         case CL_INVALID_KERNEL_DEFINITION:
 182                                 msg = "the function definition for __kernel function given by kernel_name such as the number of arguments, the argument types are not the same for all devices for which the program executable has been built.";
 183                                 break;
 184                         case CL_INVALID_VALUE:
 185                                 msg = "kernel_name is NULL.";
 186                                 break;
 187                         case CL_OUT_OF_HOST_MEMORY:
 188                                 msg = "Failure to allocate resources required by the OpenCL implementation on the host.";
 189                                 break;
 190                         default:
 191                                 {
 192                                 }
 193                         }
 194
 195                         printf("Error in clCreateKernel for kernel '%s', error is \"%s\", Line %u in file %s !!!\n\n", kernelName, msg, __LINE__, __FILE__);
 196
 197         #ifndef BT_SUPPRESS_OPENCL_ASSERTS
 198                         btAssert(0);
 199         #endif //BT_SUPPRESS_OPENCL_ASSERTS
 200                         m_kernelCompilationFailures++;
 201                         return 0;
 202                 }
 203
 204                 printf("ready. \n");
 205                 if (!kernel)
 206                         m_kernelCompilationFailures++;
 207                 return kernel;
 208         }
 209
 210 };
 211
 212
 213 #endif //USE_AMD_OPENCL
 214
 215 //
 216 void SoftDemo::createStack( btCollisionShape* boxShape, float halfCubeSize, int size, float zPos )
 217 {
 218         btTransform trans;
 219         trans.setIdentity();
 220
 221         for(int i=0; i<size; i++)
 222         {
 223                 // This constructs a row, from left to right
 224                 int rowSize = size - i;
 225                 for(int j=0; j< rowSize; j++)
 226                 {
 227                         btVector3 pos;
 228                         pos.setValue(
 229                                 -rowSize * halfCubeSize + halfCubeSize + j * 2.0f * halfCubeSize,
 230                                 halfCubeSize + i * halfCubeSize * 2.0f,
 231                                 zPos);
 232
 233                         trans.setOrigin(pos);
 234                         btScalar mass = 1.f;
 235
 236                         btRigidBody* body = 0;
 237                         body = localCreateRigidBody(mass,trans,boxShape);
 238
 239                 }
 240         }
 241 }
 242
 243
 244 ////////////////////////////////////
 245
 246 extern int gNumManifold;
 247 extern int gOverlappingPairs;
 248
 249 ///for mouse picking
 250 void pickingPreTickCallback (btDynamicsWorld *world, btScalar timeStep)
 251 {
 252         SoftDemo* softDemo = (SoftDemo*)world->getWorldUserInfo();
 253
 254         if(softDemo->m_drag)
 255         {
 256                 const int                               x=softDemo->m_lastmousepos[0];
 257                 const int                               y=softDemo->m_lastmousepos[1];
 258                 const btVector3                 rayFrom=softDemo->getCameraPosition();
 259                 const btVector3                 rayTo=softDemo->getRayTo(x,y);
 260                 const btVector3                 rayDir=(rayTo-rayFrom).normalized();
 261                 const btVector3                 N=(softDemo->getCameraTargetPosition()-softDemo->getCameraPosition()).normalized();
 262                 const btScalar                  O=btDot(softDemo->m_impact,N);
 263                 const btScalar                  den=btDot(N,rayDir);
 264                 if((den*den)>0)
 265                 {
 266                         const btScalar                  num=O-btDot(N,rayFrom);
 267                         const btScalar                  hit=num/den;
 268                         if((hit>0)&&(hit<1500))
 269                         {
 270                                 softDemo->m_goal=rayFrom+rayDir*hit;
 271                         }
 272                 }
 273                 btVector3                               delta=softDemo->m_goal-softDemo->m_node->m_x;
 274                 static const btScalar   maxdrag=10;
 275                 if(delta.length2()>(maxdrag*maxdrag))
 276                 {
 277                         delta=delta.normalized()*maxdrag;
 278                 }
 279                 softDemo->m_node->m_v+=delta/timeStep;
 280         }
 281
 282 }
 283
 284
 285
 286 void SoftDemo::displayCallback(void) {
 287
 288         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
 289
 290
 291         renderme();
 292
 293         glFlush();
 294         swapBuffers();
 295 }
 296
 297
 298 //
 299 // ImplicitShape
 300 //
 301
 302 //
 303 struct  ImplicitSphere : btSoftBody::ImplicitFn
 304 {
 305         btVector3       center;
 306         btScalar        sqradius;
 307         ImplicitSphere() {}
 308         ImplicitSphere(const btVector3& c,btScalar r) : center(c),sqradius(r*r) {}
 309         btScalar        Eval(const btVector3& x)
 310         {
 311                 return((x-center).length2()-sqradius);
 312         }
 313 };
 314
 315 //
 316 // Tetra meshes
 317 //
 318
 319 struct  TetraBunny
 320 {
 321 #include "bunny.inl"
 322 };
 323
 324 struct  TetraCube
 325 {
 326 #include "cube.inl"
 327 };
 328
 329
 330 //
 331 // Random
 332 //
 333
 334 static inline btScalar  UnitRand()
 335 {
 336         return(rand()/(btScalar)RAND_MAX);
 337 }
 338
 339 static inline btScalar  SignedUnitRand()
 340 {
 341         return(UnitRand()*2-1);
 342 }
 343
 344 static inline btVector3 Vector3Rand()
 345 {
 346         const btVector3 p=btVector3(SignedUnitRand(),SignedUnitRand(),SignedUnitRand());
 347         return(p.normalized());
 348 }
 349
 350 //
 351 // Rb rain
 352 //
 353 static void     Ctor_RbUpStack(SoftDemo* pdemo,int count)
 354 {
 355         float                           mass=10;
 356
 357         btCompoundShape* cylinderCompound = new btCompoundShape;
 358         btCollisionShape* cylinderShape = new btCylinderShapeX(btVector3(4,1,1));
 359         btCollisionShape* boxShape = new btBoxShape(btVector3(4,1,1));
 360         btTransform localTransform;
 361         localTransform.setIdentity();
 362         cylinderCompound->addChildShape(localTransform,boxShape);
 363         btQuaternion orn(SIMD_HALF_PI,0,0);
 364         localTransform.setRotation(orn);
 365         //      localTransform.setOrigin(btVector3(1,1,1));
 366         cylinderCompound->addChildShape(localTransform,cylinderShape);
 367
 368
 369         btCollisionShape*       shape[]={cylinderCompound,
 370                 new btBoxShape(btVector3(1,1,1)),
 371                 new btSphereShape(1.5)
 372
 373         };
 374         static const int        nshapes=sizeof(shape)/sizeof(shape[0]);
 375         for(int i=0;i<count;++i)
 376         {
 377                 btTransform startTransform;
 378                 startTransform.setIdentity();
 379                 startTransform.setOrigin(btVector3(0,2+6*i,0));
 380                 pdemo->localCreateRigidBody(mass,startTransform,shape[i%nshapes]);
 381                 //pdemo->localCreateRigidBody(mass,startTransform,shape[0]);
 382         }
 383 }
 384
 385 //
 386 // Big ball
 387 //
 388 static void     Ctor_BigBall(SoftDemo* pdemo,btScalar mass=10)
 389 {
 390         btTransform startTransform;
 391         startTransform.setIdentity();
 392         startTransform.setOrigin(btVector3(0,13,0));
 393         pdemo->localCreateRigidBody(mass,startTransform,new btSphereShape(3));
 394 }
 395
 396 //
 397 // Big plate
 398 //
 399 static btRigidBody*     Ctor_BigPlate(SoftDemo* pdemo,btScalar mass=15,btScalar height=4)
 400 {
 401         btTransform startTransform;
 402         startTransform.setIdentity();
 403         startTransform.setOrigin(btVector3(0,height,0.5));
 404         btRigidBody*            body=pdemo->localCreateRigidBody(mass,startTransform,new btBoxShape(btVector3(5,1,5)));
 405         body->setFriction(1);
 406         return(body);
 407 }
 408
 409 //
 410 // Linear stair
 411 //
 412 static void Ctor_LinearStair(SoftDemo* pdemo,const btVector3& org,const btVector3& sizes,btScalar angle,int count)
 413 {
 414         btBoxShape*     shape=new btBoxShape(sizes);
 415         for(int i=0;i<count;++i)
 416         {
 417                 btTransform startTransform;
 418                 startTransform.setIdentity();
 419                 startTransform.setOrigin(org+btVector3(sizes.x()*i*2,sizes.y()*i*2,0));
 420                 btRigidBody* body=pdemo->localCreateRigidBody(0,startTransform,shape);
 421                 body->setFriction(1);
 422         }
 423 }
 424
 425 //
 426 // Softbox
 427 //
 428 static btSoftBody* Ctor_SoftBox(SoftDemo* pdemo,const btVector3& p,const btVector3& s)
 429 {
 430         const btVector3 h=s*0.5;
 431         const btVector3 c[]={   p+h*btVector3(-1,-1,-1),
 432                 p+h*btVector3(+1,-1,-1),
 433                 p+h*btVector3(-1,+1,-1),
 434                 p+h*btVector3(+1,+1,-1),
 435                 p+h*btVector3(-1,-1,+1),
 436                 p+h*btVector3(+1,-1,+1),
 437                 p+h*btVector3(-1,+1,+1),
 438                 p+h*btVector3(+1,+1,+1)};
 439         btSoftBody*             psb=btSoftBodyHelpers::CreateFromConvexHull(pdemo->m_softBodyWorldInfo,c,8);
 440         psb->generateBendingConstraints(2);
 441         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 442
 443         return(psb);
 444
 445 }
 446
 447 //
 448 // SoftBoulder
 449 //
 450 static btSoftBody* Ctor_SoftBoulder(SoftDemo* pdemo,const btVector3& p,const btVector3& s,int np,int id)
 451 {
 452         btAlignedObjectArray<btVector3> pts;
 453         if(id) srand(id);
 454         for(int i=0;i<np;++i)
 455         {
 456                 pts.push_back(Vector3Rand()*s+p);
 457         }
 458         btSoftBody*             psb=btSoftBodyHelpers::CreateFromConvexHull(pdemo->m_softBodyWorldInfo,&pts[0],pts.size());
 459         psb->generateBendingConstraints(2);
 460         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 461
 462         return(psb);
 463 }
 464
 465 //#define TRACEDEMO { pdemo->demoname=__FUNCTION__+5;printf("Launching demo: " __FUNCTION__ "\r\n"); }
 466
 467 //
 468 // Basic ropes
 469 //
 470 static void     Init_Ropes(SoftDemo* pdemo)
 471 {
 472         //TRACEDEMO
 473         const int n=15;
 474         for(int i=0;i<n;++i)
 475         {
 476                 btSoftBody*     psb=btSoftBodyHelpers::CreateRope(pdemo->m_softBodyWorldInfo,   btVector3(-10,0,i*0.25),
 477                         btVector3(10,0,i*0.25),
 478                         16,
 479                         1+2);
 480                 psb->m_cfg.piterations          =       4;
 481                 psb->m_materials[0]->m_kLST     =       0.1+(i/(btScalar)(n-1))*0.9;
 482                 psb->setTotalMass(20);
 483                 pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 484
 485         }
 486 }
 487
 488 //
 489 // Rope attach
 490 //
 491 static void     Init_RopeAttach(SoftDemo* pdemo)
 492 {
 493         //TRACEDEMO
 494         pdemo->m_softBodyWorldInfo.m_sparsesdf.RemoveReferences(0);
 495         struct  Functors
 496         {
 497                 static btSoftBody* CtorRope(SoftDemo* pdemo,const btVector3& p)
 498                 {
 499                         btSoftBody*     psb=btSoftBodyHelpers::CreateRope(pdemo->m_softBodyWorldInfo,p,p+btVector3(10,0,0),8,1);
 500                         psb->setTotalMass(50);
 501                         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 502                         return(psb);
 503                 }
 504         };
 505         btTransform startTransform;
 506         startTransform.setIdentity();
 507         startTransform.setOrigin(btVector3(12,8,0));
 508         btRigidBody*            body=pdemo->localCreateRigidBody(50,startTransform,new btBoxShape(btVector3(2,6,2)));
 509         btSoftBody*     psb0=Functors::CtorRope(pdemo,btVector3(0,8,-1));
 510         btSoftBody*     psb1=Functors::CtorRope(pdemo,btVector3(0,8,+1));
 511         psb0->appendAnchor(psb0->m_nodes.size()-1,body);
 512         psb1->appendAnchor(psb1->m_nodes.size()-1,body);
 513 }
 514
 515 //
 516 // Cloth attach
 517 //
 518 static void     Init_ClothAttach(SoftDemo* pdemo)
 519 {
 520         //TRACEDEMO
 521         const btScalar  s=4;
 522         const btScalar  h=6;
 523         const int               r=9;
 524         btSoftBody*             psb=btSoftBodyHelpers::CreatePatch(pdemo->m_softBodyWorldInfo,btVector3(-s,h,-s),
 525                 btVector3(+s,h,-s),
 526                 btVector3(-s,h,+s),
 527                 btVector3(+s,h,+s),r,r,4+8,true);
 528         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 529
 530         btTransform startTransform;
 531         startTransform.setIdentity();
 532         startTransform.setOrigin(btVector3(0,h,-(s+3.5)));
 533         btRigidBody*            body=pdemo->localCreateRigidBody(20,startTransform,new btBoxShape(btVector3(s,1,3)));
 534         psb->appendAnchor(0,body);
 535         psb->appendAnchor(r-1,body);
 536         pdemo->m_cutting=true;
 537 }
 538
 539 //
 540 // Impact
 541 //
 542 static void     Init_Impact(SoftDemo* pdemo)
 543 {
 544         //TRACEDEMO
 545         btSoftBody*     psb=btSoftBodyHelpers::CreateRope(pdemo->m_softBodyWorldInfo,   btVector3(0,0,0),
 546                 btVector3(0,-1,0),
 547                 0,
 548                 1);
 549         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 550         psb->m_cfg.kCHR=0.5;
 551         btTransform startTransform;
 552         startTransform.setIdentity();
 553         startTransform.setOrigin(btVector3(0,20,0));
 554         pdemo->localCreateRigidBody(10,startTransform,new btBoxShape(btVector3(2,2,2)));
 555 }
 556
 557 static void     Init_CapsuleCollision(SoftDemo* pdemo)
 558 {
 559 #ifdef USE_AMD_OPENCL
 560         btAlignedObjectArray<btSoftBody*> emptyArray;
 561         if (g_openCLSIMDSolver)
 562                 g_openCLSIMDSolver->optimize(emptyArray);
 563 #endif //USE_AMD_OPENCL
 564
 565                 //TRACEDEMO
 566         const btScalar  s=4;
 567         const btScalar  h=6;
 568         const int               r=20;
 569
 570         btTransform startTransform;
 571         startTransform.setIdentity();
 572         startTransform.setOrigin(btVector3(0,h-2,0));
 573
 574         btCollisionShape* capsuleShape= new btCapsuleShapeX(1,5);
 575         capsuleShape->setMargin( 0.5 );
 576
 577         //      capsule->setLocalScaling(btVector3(5,1,1));
 578 //      btRigidBody*            body=pdemo->localCreateRigidBody(20,startTransform,capsuleShape);
 579         btRigidBody*            body=pdemo->localCreateRigidBody(0,startTransform,capsuleShape);
 580         body->setFriction( 0.8f );
 581
 582         int fixed=0;//4+8;
 583         btSoftBody*             psb=btSoftBodyHelpers::CreatePatch(pdemo->m_softBodyWorldInfo,btVector3(-s,h,-s),
 584                 btVector3(+s,h,-s),
 585                 btVector3(-s,h,+s),
 586                 btVector3(+s,h,+s),r,r,fixed,true);
 587         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 588         psb->setTotalMass(0.1);
 589
 590         psb->m_cfg.piterations = 10;
 591         psb->m_cfg.citerations = 10;
 592         psb->m_cfg.diterations = 10;
 593 //      psb->m_cfg.viterations = 10;
 594
 595
 596         //      psb->appendAnchor(0,body);
 597 //      psb->appendAnchor(r-1,body);
 598 //      pdemo->m_cutting=true;
 599 }
 600
 601 //
 602 // Collide
 603 //
 604 static void     Init_Collide(SoftDemo* pdemo)
 605 {
 606         //TRACEDEMO
 607         struct Functor
 608         {
 609                 static btSoftBody* Create(SoftDemo* pdemo,const btVector3& x,const btVector3& a)
 610                 {
 611                         btSoftBody*     psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVertices,
 612                                 &gIndices[0][0],
 613                                 NUM_TRIANGLES);
 614                         psb->generateBendingConstraints(2);
 615                         psb->m_cfg.piterations=2;
 616                         psb->m_cfg.collisions|=btSoftBody::fCollision::VF_SS;
 617                         psb->randomizeConstraints();
 618                         btMatrix3x3     m;
 619                         m.setEulerZYX(a.x(),a.y(),a.z());
 620                         psb->transform(btTransform(m,x));
 621                         psb->scale(btVector3(2,2,2));
 622                         psb->setTotalMass(50,true);
 623                         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 624                         return(psb);
 625                 }
 626         };
 627         for(int i=0;i<3;++i)
 628         {
 629                 Functor::Create(pdemo,btVector3(3*i,2,0),btVector3(SIMD_PI/2*(1-(i&1)),SIMD_PI/2*(i&1),0));
 630         }
 631         pdemo->m_cutting=true;
 632 }
 633
 634 //
 635 // Collide2
 636 //
 637 static void     Init_Collide2(SoftDemo* pdemo)
 638 {
 639         //TRACEDEMO
 640         struct Functor
 641         {
 642                 static btSoftBody* Create(SoftDemo* pdemo,const btVector3& x,const btVector3& a)
 643                 {
 644                         btSoftBody*     psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVerticesBunny,
 645                                 &gIndicesBunny[0][0],
 646                                 BUNNY_NUM_TRIANGLES);
 647                         btSoftBody::Material*   pm=psb->appendMaterial();
 648                         pm->m_kLST                              =       0.5;
 649                         pm->m_flags                             -=      btSoftBody::fMaterial::DebugDraw;
 650                         psb->generateBendingConstraints(2,pm);
 651                         psb->m_cfg.piterations  =       2;
 652                         psb->m_cfg.kDF                  =       0.5;
 653                         psb->m_cfg.collisions   |=      btSoftBody::fCollision::VF_SS;
 654                         psb->randomizeConstraints();
 655                         btMatrix3x3     m;
 656                         m.setEulerZYX(a.x(),a.y(),a.z());
 657                         psb->transform(btTransform(m,x));
 658                         psb->scale(btVector3(6,6,6));
 659                         psb->setTotalMass(100,true);
 660                         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 661                         return(psb);
 662                 }
 663         };
 664         for(int i=0;i<3;++i)
 665         {
 666                 Functor::Create(pdemo,btVector3(0,-1+5*i,0),btVector3(0,SIMD_PI/2*(i&1),0));
 667         }
 668         pdemo->m_cutting=true;
 669 }
 670
 671 //
 672 // Collide3
 673 //
 674 static void     Init_Collide3(SoftDemo* pdemo)
 675 {
 676         //TRACEDEMO
 677         {
 678                 const btScalar  s=8;
 679                 btSoftBody*             psb=btSoftBodyHelpers::CreatePatch(     pdemo->m_softBodyWorldInfo,btVector3(-s,0,-s),
 680                         btVector3(+s,0,-s),
 681                         btVector3(-s,0,+s),
 682                         btVector3(+s,0,+s),
 683                         15,15,1+2+4+8,true);
 684                 psb->m_materials[0]->m_kLST     =       0.4;
 685                 psb->m_cfg.collisions           |=      btSoftBody::fCollision::VF_SS;
 686                 psb->setTotalMass(150);
 687                 pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 688         }
 689         {
 690                 const btScalar  s=4;
 691                 const btVector3 o=btVector3(5,10,0);
 692                 btSoftBody*             psb=btSoftBodyHelpers::CreatePatch(     pdemo->m_softBodyWorldInfo,
 693                         btVector3(-s,0,-s)+o,
 694                         btVector3(+s,0,-s)+o,
 695                         btVector3(-s,0,+s)+o,
 696                         btVector3(+s,0,+s)+o,
 697                         7,7,0,true);
 698                 btSoftBody::Material*   pm=psb->appendMaterial();
 699                 pm->m_kLST                              =       0.1;
 700                 pm->m_flags                             -=      btSoftBody::fMaterial::DebugDraw;
 701                 psb->generateBendingConstraints(2,pm);
 702                 psb->m_materials[0]->m_kLST     =       0.5;
 703                 psb->m_cfg.collisions           |=      btSoftBody::fCollision::VF_SS;
 704                 psb->setTotalMass(150);
 705                 pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 706                 pdemo->m_cutting=true;
 707         }
 708 }
 709
 710 //
 711 // Aerodynamic forces, 50x1g flyers
 712 //
 713 static void     Init_Aero(SoftDemo* pdemo)
 714 {
 715         //TRACEDEMO
 716         const btScalar  s=2;
 717         const btScalar  h=10;
 718         const int               segments=6;
 719         const int               count=50;
 720         for(int i=0;i<count;++i)
 721         {
 722                 btSoftBody*             psb=btSoftBodyHelpers::CreatePatch(pdemo->m_softBodyWorldInfo,btVector3(-s,h,-s),
 723                         btVector3(+s,h,-s),
 724                         btVector3(-s,h,+s),
 725                         btVector3(+s,h,+s),
 726                         segments,segments,
 727                         0,true);
 728                 btSoftBody::Material*   pm=psb->appendMaterial();
 729                 pm->m_flags                             -=      btSoftBody::fMaterial::DebugDraw;
 730                 psb->generateBendingConstraints(2,pm);
 731                 psb->m_cfg.kLF                  =       0.004;
 732                 psb->m_cfg.kDG                  =       0.0003;
 733                 psb->m_cfg.aeromodel    =       btSoftBody::eAeroModel::V_TwoSided;
 734                 btTransform             trs;
 735                 btQuaternion    rot;
 736                 btVector3               ra=Vector3Rand()*0.1;
 737                 btVector3               rp=Vector3Rand()*15+btVector3(0,20,80);
 738                 rot.setEuler(SIMD_PI/8+ra.x(),-SIMD_PI/7+ra.y(),ra.z());
 739                 trs.setIdentity();
 740                 trs.setOrigin(rp);
 741                 trs.setRotation(rot);
 742                 psb->transform(trs);
 743                 psb->setTotalMass(0.1);
 744                 psb->addForce(btVector3(0,2,0),0);
 745                 pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 746
 747         }
 748         pdemo->m_autocam=true;
 749 }
 750
 751 static void     Init_Aero2(SoftDemo* pdemo)
 752 {
 753         //TRACEDEMO
 754         const btScalar  s=5;
 755         const int               segments=10;
 756         const int               count=5;
 757         btVector3 pos(-s*segments, 0, 0);
 758         btScalar gap = 0.5;
 759
 760         for(int i=0;i<count;++i)
 761         {
 762                 btSoftBody*             psb=btSoftBodyHelpers::CreatePatch(     pdemo->m_softBodyWorldInfo,btVector3(-s,0,-s*3),
 763                         btVector3(+s,0,-s*3),
 764                         btVector3(-s,0,+s),
 765                         btVector3(+s,0,+s),
 766                         segments,segments*3,
 767                         1+2,true);
 768
 769                 psb->getCollisionShape()->setMargin(0.5);
 770                 btSoftBody::Material* pm=psb->appendMaterial();
 771                 pm->m_kLST              =       0.0004;
 772                 pm->m_flags             -=      btSoftBody::fMaterial::DebugDraw;
 773                 psb->generateBendingConstraints(2,pm);
 774
 775                 psb->m_cfg.kLF                  =       0.05;
 776                 psb->m_cfg.kDG                  =       0.01;
 777
 778                 //psb->m_cfg.kLF                        =       0.004;
 779                 //psb->m_cfg.kDG                        =       0.0003;
 780
 781                 psb->m_cfg.piterations = 2;
 782                 psb->m_cfg.aeromodel    =       btSoftBody::eAeroModel::V_TwoSidedLiftDrag;
 783
 784
 785                 psb->setWindVelocity(btVector3(4, -12.0, -25.0));
 786
 787                 btTransform             trs;
 788                 btQuaternion    rot;
 789                 pos += btVector3(s*2 + gap, 0, 0);
 790                 rot.setRotation(btVector3(1, 0, 0), btScalar(SIMD_PI/2));
 791                 trs.setIdentity();
 792                 trs.setOrigin(pos);
 793                 trs.setRotation(rot);
 794                 psb->transform(trs);
 795                 psb->setTotalMass(2.0);
 796
 797                 pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 798         }
 799
 800         pdemo->m_autocam=true;
 801 }
 802
 803 //
 804 // Friction
 805 //
 806 static void     Init_Friction(SoftDemo* pdemo)
 807 {
 808         //TRACEDEMO
 809         const btScalar  bs=2;
 810         const btScalar  ts=bs+bs/4;
 811         for(int i=0,ni=20;i<ni;++i)
 812         {
 813                 const btVector3 p(-ni*ts/2+i*ts,-10+bs,40);
 814                 btSoftBody*             psb=Ctor_SoftBox(pdemo,p,btVector3(bs,bs,bs));
 815                 psb->m_cfg.kDF  =       0.1 * ((i+1)/(btScalar)ni);
 816                 psb->addVelocity(btVector3(0,0,-10));
 817         }
 818 }
 819
 820 //
 821 // Pressure
 822 //
 823 static void     Init_Pressure(SoftDemo* pdemo)
 824 {
 825         //TRACEDEMO
 826         btSoftBody*     psb=btSoftBodyHelpers::CreateEllipsoid(pdemo->m_softBodyWorldInfo,btVector3(35,25,0),
 827                 btVector3(1,1,1)*3,
 828                 512);
 829         psb->m_materials[0]->m_kLST     =       0.1;
 830         psb->m_cfg.kDF                          =       1;
 831         psb->m_cfg.kDP                          =       0.001; // fun factor...
 832         psb->m_cfg.kPR                          =       2500;
 833         psb->setTotalMass(30,true);
 834         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 835
 836         Ctor_BigPlate(pdemo);
 837         Ctor_LinearStair(pdemo,btVector3(0,0,0),btVector3(2,1,5),0,10);
 838         pdemo->m_autocam=true;
 839
 840 }
 841
 842 //
 843 // Volume conservation
 844 //
 845 static void     Init_Volume(SoftDemo* pdemo)
 846 {
 847         //TRACEDEMO
 848         btSoftBody*     psb=btSoftBodyHelpers::CreateEllipsoid(pdemo->m_softBodyWorldInfo,btVector3(35,25,0),
 849                 btVector3(1,1,1)*3,
 850                 512);
 851         psb->m_materials[0]->m_kLST     =       0.45;
 852         psb->m_cfg.kVC                          =       20;
 853         psb->setTotalMass(50,true);
 854         psb->setPose(true,false);
 855         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 856
 857         Ctor_BigPlate(pdemo);
 858         Ctor_LinearStair(pdemo,btVector3(0,0,0),btVector3(2,1,5),0,10);
 859         pdemo->m_autocam=true;
 860
 861 }
 862
 863 //
 864 // Stick+Bending+Rb's
 865 //
 866 static void     Init_Sticks(SoftDemo* pdemo)
 867 {
 868         //TRACEDEMO
 869         const int               n=16;
 870         const int               sg=4;
 871         const btScalar  sz=5;
 872         const btScalar  hg=4;
 873         const btScalar  in=1/(btScalar)(n-1);
 874         for(int y=0;y<n;++y)
 875         {
 876                 for(int x=0;x<n;++x)
 877                 {
 878                         const btVector3 org(-sz+sz*2*x*in,
 879                                 -10,
 880                                 -sz+sz*2*y*in);
 881                         btSoftBody*             psb=btSoftBodyHelpers::CreateRope(      pdemo->m_softBodyWorldInfo,     org,
 882                                 org+btVector3(hg*0.001,hg,0),
 883                                 sg,
 884                                 1);
 885                         psb->m_cfg.kDP          =       0.005;
 886                         psb->m_cfg.kCHR         =       0.1;
 887                         for(int i=0;i<3;++i)
 888                         {
 889                                 psb->generateBendingConstraints(2+i);
 890                         }
 891                         psb->setMass(1,0);
 892                         psb->setTotalMass(0.01);
 893                         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 894
 895                 }
 896         }
 897         Ctor_BigBall(pdemo);
 898 }
 899
 900 //
 901 // Bending
 902 //
 903 static void     Init_Bending(SoftDemo* pdemo)
 904 {
 905         //TRACEDEMO
 906         const btScalar  s=4;
 907         const btVector3 x[]={   btVector3(-s,0,-s),
 908                 btVector3(+s,0,-s),
 909                 btVector3(+s,0,+s),
 910                 btVector3(-s,0,+s)};
 911         const btScalar  m[]={   0,0,0,1};
 912         btSoftBody*             psb=new btSoftBody(&pdemo->m_softBodyWorldInfo,4,x,m);
 913         psb->appendLink(0,1);
 914         psb->appendLink(1,2);
 915         psb->appendLink(2,3);
 916         psb->appendLink(3,0);
 917         psb->appendLink(0,2);
 918
 919         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 920 }
 921
 922 //
 923 // 100kg cloth locked at corners, 10 falling 10kg rb's.
 924 //
 925 static void     Init_Cloth(SoftDemo* pdemo)
 926 {
 927         //TRACEDEMO
 928         const btScalar  s=8;
 929         btSoftBody*             psb=btSoftBodyHelpers::CreatePatch(     pdemo->m_softBodyWorldInfo,btVector3(-s,0,-s),
 930                 btVector3(+s,0,-s),
 931                 btVector3(-s,0,+s),
 932                 btVector3(+s,0,+s),
 933                 31,31,
 934                 //              31,31,
 935                 1+2+4+8,true);
 936
 937         psb->getCollisionShape()->setMargin(0.5);
 938         btSoftBody::Material* pm=psb->appendMaterial();
 939         pm->m_kLST              =       0.4;
 940         pm->m_flags             -=      btSoftBody::fMaterial::DebugDraw;
 941         psb->generateBendingConstraints(2,pm);
 942         psb->setTotalMass(150);
 943         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 944
 945         Ctor_RbUpStack(pdemo,10);
 946         pdemo->m_cutting=true;
 947 }
 948
 949 //
 950 // 100kg Stanford's bunny
 951 //
 952 static void     Init_Bunny(SoftDemo* pdemo)
 953 {
 954         //TRACEDEMO
 955         btSoftBody*     psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVerticesBunny,
 956                 &gIndicesBunny[0][0],
 957                 BUNNY_NUM_TRIANGLES);
 958         btSoftBody::Material*   pm=psb->appendMaterial();
 959         pm->m_kLST                              =       0.5;
 960         pm->m_flags                             -=      btSoftBody::fMaterial::DebugDraw;
 961         psb->generateBendingConstraints(2,pm);
 962         psb->m_cfg.piterations  =       2;
 963         psb->m_cfg.kDF                  =       0.5;
 964         psb->randomizeConstraints();
 965         psb->scale(btVector3(6,6,6));
 966         psb->setTotalMass(100,true);
 967         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 968         pdemo->m_cutting=true;
 969
 970 }
 971
 972 //
 973 // 100kg Stanford's bunny with pose matching
 974 //
 975 static void     Init_BunnyMatch(SoftDemo* pdemo)
 976 {
 977         //TRACEDEMO
 978         btSoftBody*     psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,    gVerticesBunny,
 979                 &gIndicesBunny[0][0],
 980                 BUNNY_NUM_TRIANGLES);
 981         psb->m_cfg.kDF                          =       0.5;
 982         psb->m_cfg.kMT                          =       0.05;
 983         psb->m_cfg.piterations          =       5;
 984         psb->randomizeConstraints();
 985         psb->scale(btVector3(6,6,6));
 986         psb->setTotalMass(100,true);
 987         psb->setPose(false,true);
 988         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
 989
 990 }
 991
 992 //
 993 // 50Kg Torus
 994 //
 995 static void     Init_Torus(SoftDemo* pdemo)
 996 {
 997         //TRACEDEMO
 998         btSoftBody*     psb=btSoftBodyHelpers::CreateFromTriMesh(       pdemo->m_softBodyWorldInfo,     gVertices,
 999                 &gIndices[0][0],
1000                 NUM_TRIANGLES);
1001         psb->generateBendingConstraints(2);
1002         psb->m_cfg.piterations=2;
1003         psb->randomizeConstraints();
1004         btMatrix3x3     m;
1005         m.setEulerZYX(SIMD_PI/2,0,0);
1006         psb->transform(btTransform(m,btVector3(0,4,0)));
1007         psb->scale(btVector3(2,2,2));
1008         psb->setTotalMass(50,true);
1009         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
1010         pdemo->m_cutting=true;
1011
1012
1013 }
1014
1015 //
1016 // 50Kg Torus with pose matching
1017 //
1018 static void     Init_TorusMatch(SoftDemo* pdemo)
1019 {
1020         //TRACEDEMO
1021         btSoftBody*     psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,    gVertices,
1022                 &gIndices[0][0],
1023                 NUM_TRIANGLES);
1024         psb->m_materials[0]->m_kLST     =       0.1;
1025         psb->m_cfg.kMT                          =       0.05;
1026         psb->randomizeConstraints();
1027         btMatrix3x3     m;
1028         m.setEulerZYX(SIMD_PI/2,0,0);
1029         psb->transform(btTransform(m,btVector3(0,4,0)));
1030         psb->scale(btVector3(2,2,2));
1031         psb->setTotalMass(50,true);
1032         psb->setPose(false,true);
1033         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
1034 }
1035
1036 //
1037 // Cutting1
1038 //
1039 static void     Init_Cutting1(SoftDemo* pdemo)
1040 {
1041         const btScalar  s=6;
1042         const btScalar  h=2;
1043         const int               r=16;
1044         const btVector3 p[]={   btVector3(+s,h,-s),
1045                 btVector3(-s,h,-s),
1046                 btVector3(+s,h,+s),
1047                 btVector3(-s,h,+s)};
1048         btSoftBody*     psb=btSoftBodyHelpers::CreatePatch(pdemo->m_softBodyWorldInfo,p[0],p[1],p[2],p[3],r,r,1+2+4+8,true);
1049         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
1050         psb->m_cfg.piterations=1;
1051         pdemo->m_cutting=true;
1052 }
1053
1054 //
1055 // Clusters
1056 //
1057
1058 //
1059 static void                     Ctor_Gear(SoftDemo* pdemo,const btVector3& pos,btScalar speed)
1060 {
1061         btTransform startTransform;
1062         startTransform.setIdentity();
1063         startTransform.setOrigin(pos);
1064         btCompoundShape*        shape=new btCompoundShape();
1065 #if 1
1066         shape->addChildShape(btTransform(btQuaternion(0,0,0)),new btBoxShape(btVector3(5,1,6)));
1067         shape->addChildShape(btTransform(btQuaternion(0,0,SIMD_HALF_PI)),new btBoxShape(btVector3(5,1,6)));
1068 #else
1069         shape->addChildShape(btTransform(btQuaternion(0,0,0)),new btCylinderShapeZ(btVector3(5,1,7)));
1070         shape->addChildShape(btTransform(btQuaternion(0,0,SIMD_HALF_PI)),new btBoxShape(btVector3(4,1,8)));
1071 #endif
1072         btRigidBody*            body=pdemo->localCreateRigidBody(10,startTransform,shape);
1073         body->setFriction(1);
1074         btDynamicsWorld*        world=pdemo->getDynamicsWorld();
1075         btHingeConstraint*      hinge=new btHingeConstraint(*body,btTransform::getIdentity());
1076         if(speed!=0) hinge->enableAngularMotor(true,speed,3);
1077         world->addConstraint(hinge);
1078 }
1079
1080 //
1081 static btSoftBody*      Ctor_ClusterBunny(SoftDemo* pdemo,const btVector3& x,const btVector3& a)
1082 {
1083         btSoftBody*     psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVerticesBunny,&gIndicesBunny[0][0],BUNNY_NUM_TRIANGLES);
1084         btSoftBody::Material*   pm=psb->appendMaterial();
1085         pm->m_kLST                              =       1;
1086         pm->m_flags                             -=      btSoftBody::fMaterial::DebugDraw;
1087         psb->generateBendingConstraints(2,pm);
1088         psb->m_cfg.piterations  =       2;
1089         psb->m_cfg.kDF                  =       1;
1090         psb->m_cfg.collisions   =       btSoftBody::fCollision::CL_SS+
1091                 btSoftBody::fCollision::CL_RS;
1092         psb->randomizeConstraints();
1093         btMatrix3x3     m;
1094         m.setEulerZYX(a.x(),a.y(),a.z());
1095         psb->transform(btTransform(m,x));
1096         psb->scale(btVector3(8,8,8));
1097         psb->setTotalMass(150,true);
1098         psb->generateClusters(1);
1099         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
1100         return(psb);
1101 }
1102
1103 //
1104 static btSoftBody*      Ctor_ClusterTorus(SoftDemo* pdemo,const btVector3& x,const btVector3& a,const btVector3& s=btVector3(2,2,2))
1105 {
1106         btSoftBody*     psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVertices,&gIndices[0][0],NUM_TRIANGLES);
1107         btSoftBody::Material*   pm=psb->appendMaterial();
1108         pm->m_kLST                              =       1;
1109         pm->m_flags                             -=      btSoftBody::fMaterial::DebugDraw;
1110         psb->generateBendingConstraints(2,pm);
1111         psb->m_cfg.piterations  =       2;
1112         psb->m_cfg.collisions   =       btSoftBody::fCollision::CL_SS+
1113                 btSoftBody::fCollision::CL_RS;
1114         psb->randomizeConstraints();
1115         psb->scale(s);
1116         psb->rotate(btQuaternion(a[0],a[1],a[2]));
1117         psb->translate(x);
1118         psb->setTotalMass(50,true);
1119         psb->generateClusters(64);
1120         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
1121         return(psb);
1122 }
1123
1124 //
1125 static struct MotorControl : btSoftBody::AJoint::IControl
1126 {
1127         MotorControl()
1128         {
1129                 goal=0;
1130                 maxtorque=0;
1131         }
1132         btScalar        Speed(btSoftBody::AJoint*,btScalar current)
1133         {
1134                 return(current+btMin(maxtorque,btMax(-maxtorque,goal-current)));
1135         }
1136         btScalar        goal;
1137         btScalar        maxtorque;
1138 }       motorcontrol;
1139
1140 //
1141 struct SteerControl : btSoftBody::AJoint::IControl
1142 {
1143         SteerControl(btScalar s)
1144         {
1145                 angle=0;
1146                 sign=s;
1147         }
1148         void            Prepare(btSoftBody::AJoint* joint)
1149         {
1150                 joint->m_refs[0][0]=btCos(angle*sign);
1151                 joint->m_refs[0][2]=btSin(angle*sign);
1152         }
1153         btScalar        Speed(btSoftBody::AJoint* joint,btScalar current)
1154         {
1155                 return(motorcontrol.Speed(joint,current));
1156         }
1157         btScalar        angle;
1158         btScalar        sign;
1159 };
1160
1161 static SteerControl     steercontrol_f(+1);
1162 static SteerControl     steercontrol_r(-1);
1163
1164 //
1165 static void     Init_ClusterDeform(SoftDemo* pdemo)
1166 {
1167         btSoftBody*             psb=Ctor_ClusterTorus(pdemo,btVector3(0,0,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI));
1168         psb->generateClusters(8);
1169         psb->m_cfg.kDF=1;
1170 }
1171
1172 //
1173 static void     Init_ClusterCollide1(SoftDemo* pdemo)
1174 {
1175         const btScalar  s=8;
1176         btSoftBody*             psb=btSoftBodyHelpers::CreatePatch(     pdemo->m_softBodyWorldInfo,btVector3(-s,0,-s),
1177                 btVector3(+s,0,-s),
1178                 btVector3(-s,0,+s),
1179                 btVector3(+s,0,+s),
1180                 17,17,//9,9,//31,31,
1181                 1+2+4+8,
1182                 true);
1183         btSoftBody::Material* pm=psb->appendMaterial();
1184         pm->m_kLST              =       0.4;
1185         pm->m_flags             -=      btSoftBody::fMaterial::DebugDraw;
1186         psb->m_cfg.kDF  =       1;
1187         psb->m_cfg.kSRHR_CL             =       1;
1188         psb->m_cfg.kSR_SPLT_CL  =       0;
1189         psb->m_cfg.collisions   =       btSoftBody::fCollision::CL_SS+
1190
1191                 btSoftBody::fCollision::CL_RS;
1192         psb->generateBendingConstraints(2,pm);
1193
1194         psb->getCollisionShape()->setMargin(0.05);
1195         psb->setTotalMass(50);
1196
1197         ///pass zero in generateClusters to create  cluster for each tetrahedron or triangle
1198         psb->generateClusters(0);
1199         //psb->generateClusters(64);
1200
1201         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
1202
1203         Ctor_RbUpStack(pdemo,10);
1204 }
1205
1206 //
1207 static void     Init_ClusterCollide2(SoftDemo* pdemo)
1208 {
1209         struct Functor
1210         {
1211                 static btSoftBody* Create(SoftDemo* pdemo,const btVector3& x,const btVector3& a)
1212                 {
1213                         btSoftBody*     psb=btSoftBodyHelpers::CreateFromTriMesh(pdemo->m_softBodyWorldInfo,gVertices,
1214                                 &gIndices[0][0],
1215                                 NUM_TRIANGLES);
1216                         btSoftBody::Material* pm=psb->appendMaterial();
1217                         pm->m_flags             -=      btSoftBody::fMaterial::DebugDraw;
1218                         psb->generateBendingConstraints(2,pm);
1219                         psb->m_cfg.piterations=2;
1220                         psb->m_cfg.kDF                  =1;
1221                         psb->m_cfg.kSSHR_CL             =1;
1222                         psb->m_cfg.kSS_SPLT_CL  =0;
1223                         psb->m_cfg.kSKHR_CL             =0.1f;
1224                         psb->m_cfg.kSK_SPLT_CL  =1;
1225                         psb->m_cfg.collisions=  btSoftBody::fCollision::CL_SS+
1226                                 btSoftBody::fCollision::CL_RS;
1227                         psb->randomizeConstraints();
1228                         btMatrix3x3     m;
1229                         m.setEulerZYX(a.x(),a.y(),a.z());
1230                         psb->transform(btTransform(m,x));
1231                         psb->scale(btVector3(2,2,2));
1232                         psb->setTotalMass(50,true);
1233                         psb->generateClusters(16);
1234                         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
1235                         return(psb);
1236                 }
1237         };
1238         for(int i=0;i<3;++i)
1239         {
1240                 Functor::Create(pdemo,btVector3(3*i,2,0),btVector3(SIMD_PI/2*(1-(i&1)),SIMD_PI/2*(i&1),0));
1241         }
1242 }
1243
1244 //
1245 static void     Init_ClusterSocket(SoftDemo* pdemo)
1246 {
1247         btSoftBody*             psb=Ctor_ClusterTorus(pdemo,btVector3(0,0,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI));
1248         btRigidBody*    prb=Ctor_BigPlate(pdemo,50,8);
1249         psb->m_cfg.kDF=1;
1250         btSoftBody::LJoint::Specs       lj;
1251         lj.position     =       btVector3(0,5,0);
1252         psb->appendLinearJoint(lj,prb);
1253 }
1254
1255 //
1256 static void     Init_ClusterHinge(SoftDemo* pdemo)
1257 {
1258         btSoftBody*             psb=Ctor_ClusterTorus(pdemo,btVector3(0,0,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI));
1259         btRigidBody*    prb=Ctor_BigPlate(pdemo,50,8);
1260         psb->m_cfg.kDF=1;
1261         btSoftBody::AJoint::Specs       aj;
1262         aj.axis =       btVector3(0,0,1);
1263         psb->appendAngularJoint(aj,prb);
1264 }
1265
1266 //
1267 static void     Init_ClusterCombine(SoftDemo* pdemo)
1268 {
1269         const btVector3 sz(2,4,2);
1270         btSoftBody*     psb0=Ctor_ClusterTorus(pdemo,btVector3(0,8,0),btVector3(SIMD_PI/2,0,SIMD_HALF_PI),sz);
1271         btSoftBody*     psb1=Ctor_ClusterTorus(pdemo,btVector3(0,8,10),btVector3(SIMD_PI/2,0,SIMD_HALF_PI),sz);
1272         btSoftBody*     psbs[]={psb0,psb1};
1273         for(int j=0;j<2;++j)
1274         {
1275                 psbs[j]->m_cfg.kDF=1;
1276                 psbs[j]->m_cfg.kDP=0;
1277                 psbs[j]->m_cfg.piterations=1;
1278                 psbs[j]->m_clusters[0]->m_matching      =       0.05;
1279                 psbs[j]->m_clusters[0]->m_ndamping      =       0.05;
1280         }
1281         btSoftBody::AJoint::Specs       aj;
1282         aj.axis         =       btVector3(0,0,1);
1283         aj.icontrol     =       &motorcontrol;
1284         psb0->appendAngularJoint(aj,psb1);
1285
1286         btSoftBody::LJoint::Specs       lj;
1287         lj.position     =       btVector3(0,8,5);
1288         psb0->appendLinearJoint(lj,psb1);
1289 }
1290
1291 //
1292 static void     Init_ClusterCar(SoftDemo* pdemo)
1293 {
1294         pdemo->setAzi(180);
1295         const btVector3         origin(100,80,0);
1296         const btQuaternion      orientation(-SIMD_PI/2,0,0);
1297         const btScalar  widthf=8;
1298         const btScalar  widthr=9;
1299         const btScalar  length=8;
1300         const btScalar  height=4;
1301         const btVector3 wheels[]=       {
1302                 btVector3(+widthf,-height,+length),     // Front left
1303                 btVector3(-widthf,-height,+length),     // Front right
1304                 btVector3(+widthr,-height,-length),     // Rear left
1305                 btVector3(-widthr,-height,-length),     // Rear right
1306         };
1307         btSoftBody*     pa=Ctor_ClusterBunny(pdemo,btVector3(0,0,0),btVector3(0,0,0));
1308         btSoftBody*     pfl=Ctor_ClusterTorus(pdemo,wheels[0],btVector3(0,0,SIMD_HALF_PI),btVector3(2,4,2));
1309         btSoftBody*     pfr=Ctor_ClusterTorus(pdemo,wheels[1],btVector3(0,0,SIMD_HALF_PI),btVector3(2,4,2));
1310         btSoftBody*     prl=Ctor_ClusterTorus(pdemo,wheels[2],btVector3(0,0,SIMD_HALF_PI),btVector3(2,5,2));
1311         btSoftBody*     prr=Ctor_ClusterTorus(pdemo,wheels[3],btVector3(0,0,SIMD_HALF_PI),btVector3(2,5,2));
1312
1313         pfl->m_cfg.kDF  =
1314                 pfr->m_cfg.kDF  =
1315                 prl->m_cfg.kDF  =
1316                 prr->m_cfg.kDF  =       1;
1317
1318         btSoftBody::LJoint::Specs       lspecs;
1319         lspecs.cfm              =       1;
1320         lspecs.erp              =       1;
1321         lspecs.position =       btVector3(0,0,0);
1322
1323         lspecs.position=wheels[0];pa->appendLinearJoint(lspecs,pfl);
1324         lspecs.position=wheels[1];pa->appendLinearJoint(lspecs,pfr);
1325         lspecs.position=wheels[2];pa->appendLinearJoint(lspecs,prl);
1326         lspecs.position=wheels[3];pa->appendLinearJoint(lspecs,prr);
1327
1328         btSoftBody::AJoint::Specs       aspecs;
1329         aspecs.cfm              =       1;
1330         aspecs.erp              =       1;
1331         aspecs.axis             =       btVector3(1,0,0);
1332
1333         aspecs.icontrol =       &steercontrol_f;
1334         pa->appendAngularJoint(aspecs,pfl);
1335         pa->appendAngularJoint(aspecs,pfr);
1336
1337         aspecs.icontrol =       &motorcontrol;
1338         pa->appendAngularJoint(aspecs,prl);
1339         pa->appendAngularJoint(aspecs,prr);
1340
1341         pa->rotate(orientation);
1342         pfl->rotate(orientation);
1343         pfr->rotate(orientation);
1344         prl->rotate(orientation);
1345         prr->rotate(orientation);
1346         pa->translate(origin);
1347         pfl->translate(origin);
1348         pfr->translate(origin);
1349         prl->translate(origin);
1350         prr->translate(origin);
1351         pfl->m_cfg.piterations  =
1352                 pfr->m_cfg.piterations  =
1353                 prl->m_cfg.piterations  =
1354                 prr->m_cfg.piterations  = 1;
1355         pfl->m_clusters[0]->m_matching  =
1356                 pfr->m_clusters[0]->m_matching  =
1357                 prl->m_clusters[0]->m_matching  =
1358                 prr->m_clusters[0]->m_matching  = 0.05;
1359         pfl->m_clusters[0]->m_ndamping  =
1360                 pfr->m_clusters[0]->m_ndamping  =
1361                 prl->m_clusters[0]->m_ndamping  =
1362                 prr->m_clusters[0]->m_ndamping  = 0.05;
1363
1364         Ctor_LinearStair(pdemo,btVector3(0,-8,0),btVector3(3,2,40),0,20);
1365         Ctor_RbUpStack(pdemo,50);
1366         pdemo->m_autocam=true;
1367
1368 }
1369
1370 //
1371 static void     Init_ClusterRobot(SoftDemo* pdemo)
1372 {
1373         struct Functor
1374         {
1375                 static btSoftBody*      CreateBall(SoftDemo* pdemo,const btVector3& pos)
1376                 {
1377                         btSoftBody*     psb=btSoftBodyHelpers::CreateEllipsoid(pdemo->m_softBodyWorldInfo,pos,btVector3(1,1,1)*3,512);
1378                         psb->m_materials[0]->m_kLST     =       0.45;
1379                         psb->m_cfg.kVC                          =       20;
1380                         psb->setTotalMass(50,true);
1381                         psb->setPose(true,false);
1382                         psb->generateClusters(1);
1383                         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
1384                         return(psb);
1385                 }
1386         };
1387         const btVector3         base=btVector3(0,25,8);
1388         btSoftBody*                     psb0=Functor::CreateBall(pdemo,base+btVector3(-8,0,0));
1389         btSoftBody*                     psb1=Functor::CreateBall(pdemo,base+btVector3(+8,0,0));
1390         btSoftBody*                     psb2=Functor::CreateBall(pdemo,base+btVector3(0,0,+8*btSqrt(2)));
1391         const btVector3         ctr=(psb0->clusterCom(0)+psb1->clusterCom(0)+psb2->clusterCom(0))/3;
1392         btCylinderShape*        pshp=new btCylinderShape(btVector3(8,1,8));
1393         btRigidBody*            prb=pdemo->localCreateRigidBody(50,btTransform(btQuaternion(0,0,0),ctr+btVector3(0,5,0)),pshp);
1394         btSoftBody::LJoint::Specs       ls;
1395         ls.erp=0.5f;
1396         ls.position=psb0->clusterCom(0);psb0->appendLinearJoint(ls,prb);
1397         ls.position=psb1->clusterCom(0);psb1->appendLinearJoint(ls,prb);
1398         ls.position=psb2->clusterCom(0);psb2->appendLinearJoint(ls,prb);
1399
1400         btBoxShape*                     pbox=new btBoxShape(btVector3(20,1,40));
1401         btRigidBody*            pgrn=pdemo->localCreateRigidBody(0,btTransform(btQuaternion(0,-SIMD_HALF_PI/2,0),btVector3(0,0,0)),pbox);
1402
1403         pdemo->m_autocam=true;
1404
1405 }
1406
1407 //
1408 static void     Init_ClusterStackSoft(SoftDemo* pdemo)
1409 {
1410         for(int i=0;i<10;++i)
1411         {
1412                 btSoftBody*     psb=Ctor_ClusterTorus(pdemo,btVector3(0,-9+8.25*i,0),btVector3(0,0,0));
1413                 psb->m_cfg.kDF=1;
1414         }
1415 }
1416
1417 //
1418 static void     Init_ClusterStackMixed(SoftDemo* pdemo)
1419 {
1420         for(int i=0;i<10;++i)
1421         {
1422                 if((i+1)&1)
1423                 {
1424                         Ctor_BigPlate(pdemo,50,-9+4.25*i);
1425                 }
1426                 else
1427                 {
1428                         btSoftBody*     psb=Ctor_ClusterTorus(pdemo,btVector3(0,-9+4.25*i,0),btVector3(0,0,0));
1429                         psb->m_cfg.kDF=1;
1430                 }
1431         }
1432 }
1433
1434
1435 //
1436 // TetraBunny
1437 //
1438 static void     Init_TetraBunny(SoftDemo* pdemo)
1439 {
1440         btSoftBody* psb=btSoftBodyHelpers::CreateFromTetGenData(pdemo->m_softBodyWorldInfo,
1441                                                                                                                         TetraBunny::getElements(),
1442                                                                                                                         0,
1443                                                                                                                         TetraBunny::getNodes(),
1444                                                                                                                         false,true,true);
1445         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
1446         psb->rotate(btQuaternion(SIMD_PI/2,0,0));
1447         psb->setVolumeMass(150);
1448         psb->m_cfg.piterations=2;
1449         //psb->m_cfg.piterations=1;
1450         pdemo->m_cutting=false;
1451         //psb->getCollisionShape()->setMargin(0.01);
1452         psb->m_cfg.collisions   =       btSoftBody::fCollision::CL_SS+  btSoftBody::fCollision::CL_RS
1453                 //+ btSoftBody::fCollision::CL_SELF
1454                 ;
1455
1456         ///pass zero in generateClusters to create  cluster for each tetrahedron or triangle
1457         psb->generateClusters(0);
1458         //psb->m_materials[0]->m_kLST=.2;
1459         psb->m_cfg.kDF  =       10. ;
1460
1461
1462 }
1463
1464 //
1465 // TetraCube
1466 //
1467 static void     Init_TetraCube(SoftDemo* pdemo)
1468 {
1469         btSoftBody* psb=btSoftBodyHelpers::CreateFromTetGenData(pdemo->m_softBodyWorldInfo,
1470                                                                                                                         TetraCube::getElements(),
1471                                                                                                                         0,
1472                                                                                                                         TetraCube::getNodes(),
1473                                                                                                                         false,true,true);
1474         pdemo->getSoftDynamicsWorld()->addSoftBody(psb);
1475         psb->scale(btVector3(4,4,4));
1476         psb->translate(btVector3(0,5,0));
1477         psb->setVolumeMass(300);
1478
1479
1480         ///fix one vertex
1481         //psb->setMass(0,0);
1482         //psb->setMass(10,0);
1483         //psb->setMass(20,0);
1484         psb->m_cfg.piterations=1;
1485         //psb->generateClusters(128);
1486         psb->generateClusters(16);
1487         //psb->getCollisionShape()->setMargin(0.5);
1488
1489         psb->getCollisionShape()->setMargin(0.01);
1490         psb->m_cfg.collisions   =       btSoftBody::fCollision::CL_SS+  btSoftBody::fCollision::CL_RS
1491                 //+ btSoftBody::fCollision::CL_SELF
1492                 ;
1493         psb->m_materials[0]->m_kLST=0.8;
1494         pdemo->m_cutting=false;
1495 }
1496
1497
1498
1499
1500
1501         /* Init         */
1502         void (*demofncs[])(SoftDemo*)=
1503         {
1504                 Init_Cloth,
1505                 Init_Pressure,
1506                 Init_Volume,
1507                 Init_Ropes,
1508                 Init_RopeAttach,
1509                 Init_ClothAttach,
1510                 Init_Sticks,
1511                 Init_CapsuleCollision,
1512                 Init_Collide,
1513                 Init_Collide2,
1514                 Init_Collide3,
1515                 Init_Impact,
1516                 Init_Aero,
1517                 Init_Aero2,
1518                 Init_Friction,
1519                 Init_Torus,
1520                 Init_TorusMatch,
1521                 Init_Bunny,
1522                 Init_BunnyMatch,
1523                 Init_Cutting1,
1524                 Init_ClusterDeform,
1525                 Init_ClusterCollide1,
1526                 Init_ClusterCollide2,
1527                 Init_ClusterSocket,
1528                 Init_ClusterHinge,
1529                 Init_ClusterCombine,
1530                 Init_ClusterCar,
1531                 Init_ClusterRobot,
1532                 Init_ClusterStackSoft,
1533                 Init_ClusterStackMixed,
1534                 Init_TetraCube,
1535                 Init_TetraBunny,
1536         };
1537
1538 void    SoftDemo::clientResetScene()
1539 {
1540         m_azi = 0;
1541         m_cameraDistance = 30.f;
1542         m_cameraTargetPosition.setValue(0,0,0);
1543
1544         DemoApplication::clientResetScene();
1545         /* Clean up     */
1546         for(int i=m_dynamicsWorld->getNumCollisionObjects()-1;i>=0;i--)
1547         {
1548                 btCollisionObject*      obj=m_dynamicsWorld->getCollisionObjectArray()[i];
1549                 btRigidBody*            body=btRigidBody::upcast(obj);
1550                 if(body&&body->getMotionState())
1551                 {
1552                         delete body->getMotionState();
1553                 }
1554                 while(m_dynamicsWorld->getNumConstraints())
1555                 {
1556                         btTypedConstraint*      pc=m_dynamicsWorld->getConstraint(0);
1557                         m_dynamicsWorld->removeConstraint(pc);
1558                         delete pc;
1559                 }
1560                 btSoftBody* softBody = btSoftBody::upcast(obj);
1561                 if (softBody)
1562                 {
1563                         getSoftDynamicsWorld()->removeSoftBody(softBody);
1564                 } else
1565                 {
1566                         btRigidBody* body = btRigidBody::upcast(obj);
1567                         if (body)
1568                                 m_dynamicsWorld->removeRigidBody(body);
1569                         else
1570                                 m_dynamicsWorld->removeCollisionObject(obj);
1571                 }
1572                 delete obj;
1573         }
1574
1575
1576         //create ground object
1577         btTransform tr;
1578         tr.setIdentity();
1579         tr.setOrigin(btVector3(0,-12,0));
1580
1581         btCollisionObject* newOb = new btCollisionObject();
1582         newOb->setWorldTransform(tr);
1583         newOb->setInterpolationWorldTransform( tr);
1584         int lastDemo = (sizeof(demofncs)/sizeof(demofncs[0]))-1;
1585
1586         if (current_demo<0)
1587                 current_demo = lastDemo;
1588         if (current_demo > lastDemo)
1589                 current_demo =0;
1590
1591
1592         if (current_demo>19)
1593         {
1594                 newOb->setCollisionShape(m_collisionShapes[0]);
1595         } else
1596         {
1597                 newOb->setCollisionShape(m_collisionShapes[1]);
1598         }
1599
1600         m_dynamicsWorld->addCollisionObject(newOb);
1601
1602         m_softBodyWorldInfo.m_sparsesdf.Reset();
1603
1604
1605
1606
1607
1608
1609
1610         motorcontrol.goal = 0;
1611         motorcontrol.maxtorque = 0;
1612
1613
1614
1615         m_softBodyWorldInfo.air_density         =       (btScalar)1.2;
1616         m_softBodyWorldInfo.water_density       =       0;
1617         m_softBodyWorldInfo.water_offset                =       0;
1618         m_softBodyWorldInfo.water_normal                =       btVector3(0,0,0);
1619         m_softBodyWorldInfo.m_gravity.setValue(0,-10,0);
1620
1621
1622         m_autocam                                               =       false;
1623         m_raycast                                               =       false;
1624         m_cutting                                               =       false;
1625         m_results.fraction                              =       1.f;
1626         demofncs[current_demo](this);
1627 }
1628
1629
1630 void SoftDemo::clientMoveAndDisplay()
1631 {
1632         glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT|GL_STENCIL_BUFFER_BIT);
1633
1634
1635
1636
1637         float ms = getDeltaTimeMicroseconds();
1638         float dt = ms / 1000000.f;//1.0/60.;
1639
1640
1641
1642         if (m_dynamicsWorld)
1643         {
1644
1645                 if (sDemoMode)
1646                 {
1647                         static float demoCounter = DEMO_MODE_TIMEOUT;
1648                         demoCounter-= dt;
1649                         if (demoCounter<0)
1650                         {
1651
1652                                 demoCounter=DEMO_MODE_TIMEOUT;
1653                                 current_demo++;
1654                                 current_demo=current_demo%(sizeof(demofncs)/sizeof(demofncs[0]));
1655                                 clientResetScene();
1656                         }
1657                 }
1658
1659
1660 //#define FIXED_STEP
1661 #ifdef FIXED_STEP
1662                 m_dynamicsWorld->stepSimulation(dt=1.0f/60.f,0);
1663
1664 #else
1665                 //during idle mode, just run 1 simulation step maximum, otherwise 4 at max
1666                 int maxSimSubSteps = m_idle ? 1 : 4;
1667                 //if (m_idle)
1668                 //      dt = 1.0/420.f;
1669
1670                 int numSimSteps;
1671                 numSimSteps = m_dynamicsWorld->stepSimulation(dt);
1672                 //numSimSteps = m_dynamicsWorld->stepSimulation(dt,10,1./240.f);
1673
1674 #ifdef VERBOSE_TIMESTEPPING_CONSOLEOUTPUT
1675                 if (!numSimSteps)
1676                         printf("Interpolated transforms\n");
1677                 else
1678                 {
1679                         if (numSimSteps > maxSimSubSteps)
1680                         {
1681                                 //detect dropping frames
1682                                 printf("Dropped (%i) simulation steps out of %i\n",numSimSteps - maxSimSubSteps,numSimSteps);
1683                         } else
1684                         {
1685                                 printf("Simulated (%i) steps\n",numSimSteps);
1686                         }
1687                 }
1688 #endif //VERBOSE_TIMESTEPPING_CONSOLEOUTPUT
1689
1690 #endif
1691
1692 #ifdef USE_AMD_OPENCL
1693                 if (g_openCLSIMDSolver)
1694                         g_openCLSIMDSolver->copyBackToSoftBodies();
1695 #endif //USE_AMD_OPENCL
1696
1697                 if(m_drag)
1698                 {
1699                         m_node->m_v*=0;
1700                 }
1701
1702                 m_softBodyWorldInfo.m_sparsesdf.GarbageCollect();
1703
1704                 //optional but useful: debug drawing
1705
1706         }
1707
1708 #ifdef USE_QUICKPROF
1709         btProfiler::beginBlock("render");
1710 #endif //USE_QUICKPROF
1711
1712         renderme();
1713
1714         //render the graphics objects, with center of mass shift
1715
1716         updateCamera();
1717
1718
1719
1720 #ifdef USE_QUICKPROF
1721         btProfiler::endBlock("render");
1722 #endif
1723         glFlush();
1724         //some additional debugging info
1725 #ifdef PRINT_CONTACT_STATISTICS
1726         printf("num manifolds: %i\n",gNumManifold);
1727         printf("num gOverlappingPairs: %i\n",gOverlappingPairs);
1728
1729 #endif //PRINT_CONTACT_STATISTICS
1730
1731
1732         swapBuffers();
1733
1734 }
1735
1736
1737
1738 void    SoftDemo::renderme()
1739 {
1740         btIDebugDraw*   idraw=m_dynamicsWorld->getDebugDrawer();
1741
1742         glDisable(GL_TEXTURE_2D);
1743         glDisable(GL_LIGHTING);
1744         m_dynamicsWorld->debugDrawWorld();
1745
1746         int debugMode = m_dynamicsWorld->getDebugDrawer()? m_dynamicsWorld->getDebugDrawer()->getDebugMode() : -1;
1747
1748         btSoftRigidDynamicsWorld* softWorld = (btSoftRigidDynamicsWorld*)m_dynamicsWorld;
1749         btIDebugDraw*   sdraw = softWorld ->getDebugDrawer();
1750
1751
1752         for (  int i=0;i<softWorld->getSoftBodyArray().size();i++)
1753         {
1754                 btSoftBody*     psb=(btSoftBody*)softWorld->getSoftBodyArray()[i];
1755                 if (softWorld->getDebugDrawer() && !(softWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
1756                 {
1757                         btSoftBodyHelpers::DrawFrame(psb,softWorld->getDebugDrawer());
1758                         btSoftBodyHelpers::Draw(psb,softWorld->getDebugDrawer(),softWorld->getDrawFlags());
1759                 }
1760         }
1761
1762         /* Bodies               */
1763         btVector3       ps(0,0,0);
1764         int                     nps=0;
1765
1766         btSoftBodyArray&        sbs=getSoftDynamicsWorld()->getSoftBodyArray();
1767         for(int ib=0;ib<sbs.size();++ib)
1768         {
1769                 btSoftBody*     psb=sbs[ib];
1770                 nps+=psb->m_nodes.size();
1771                 for(int i=0;i<psb->m_nodes.size();++i)
1772                 {
1773                         ps+=psb->m_nodes[i].m_x;
1774                 }
1775         }
1776         ps/=nps;
1777         if(m_autocam)
1778                 m_cameraTargetPosition+=(ps-m_cameraTargetPosition)*0.05;
1779         /* Anm                  */
1780         if(!isIdle())
1781                 m_animtime=m_clock.getTimeMilliseconds()/1000.f;
1782         /* Ray cast             */
1783         if(m_raycast)
1784         {
1785                 /* Prepare rays */
1786                 const int               res=64;
1787                 const btScalar  fres=res-1;
1788                 const btScalar  size=8;
1789                 const btScalar  dist=10;
1790                 btTransform             trs;
1791                 trs.setOrigin(ps);
1792                 btScalar rayLength = 1000.f;
1793
1794                 const btScalar  angle=m_animtime*0.2;
1795                 trs.setRotation(btQuaternion(angle,SIMD_PI/4,0));
1796                 btVector3       dir=trs.getBasis()*btVector3(0,-1,0);
1797                 trs.setOrigin(ps-dir*dist);
1798                 btAlignedObjectArray<btVector3> origins;
1799                 btAlignedObjectArray<btScalar>  fractions;
1800                 origins.resize(res*res);
1801                 fractions.resize(res*res,1.f);
1802                 for(int y=0;y<res;++y)
1803                 {
1804                         for(int x=0;x<res;++x)
1805                         {
1806                                 const int       idx=y*res+x;
1807                                 origins[idx]=trs*btVector3(-size+size*2*x/fres,dist,-size+size*2*y/fres);
1808                         }
1809                 }
1810                 /* Cast rays    */
1811                 {
1812                         m_clock.reset();
1813                         if (sbs.size())
1814                         {
1815                                 btVector3*              org=&origins[0];
1816                                 btScalar*                               fraction=&fractions[0];
1817                                 btSoftBody**                    psbs=&sbs[0];
1818                                 btSoftBody::sRayCast    results;
1819                                 for(int i=0,ni=origins.size(),nb=sbs.size();i<ni;++i)
1820                                 {
1821                                         for(int ib=0;ib<nb;++ib)
1822                                         {
1823                                                 btVector3 rayFrom = *org;
1824                                                 btVector3 rayTo = rayFrom+dir*rayLength;
1825                                                 if(psbs[ib]->rayTest(rayFrom,rayTo,results))
1826                                                 {
1827                                                         *fraction=results.fraction;
1828                                                 }
1829                                         }
1830                                         ++org;++fraction;
1831                                 }
1832                                 long    ms=btMax<long>(m_clock.getTimeMilliseconds(),1);
1833                                 long    rayperseconds=(1000*(origins.size()*sbs.size()))/ms;
1834                                 printf("%d ms (%d rays/s)\r\n",int(ms),int(rayperseconds));
1835                         }
1836                 }
1837                 /* Draw rays    */
1838                 const btVector3 c[]={   origins[0],
1839                         origins[res-1],
1840                         origins[res*(res-1)],
1841                         origins[res*(res-1)+res-1]};
1842                 idraw->drawLine(c[0],c[1],btVector3(0,0,0));
1843                 idraw->drawLine(c[1],c[3],btVector3(0,0,0));
1844                 idraw->drawLine(c[3],c[2],btVector3(0,0,0));
1845                 idraw->drawLine(c[2],c[0],btVector3(0,0,0));
1846                 for(int i=0,ni=origins.size();i<ni;++i)
1847                 {
1848                         const btScalar          fraction=fractions[i];
1849                         const btVector3&        org=origins[i];
1850                         if(fraction<1.f)
1851                         {
1852                                 idraw->drawLine(org,org+dir*rayLength*fraction,btVector3(1,0,0));
1853                         }
1854                         else
1855                         {
1856                                 idraw->drawLine(org,org-dir*rayLength*0.1,btVector3(0,0,0));
1857                         }
1858                 }
1859 #undef RES
1860         }
1861         /* Water level  */
1862         static const btVector3  axis[]={btVector3(1,0,0),
1863                 btVector3(0,1,0),
1864                 btVector3(0,0,1)};
1865         if(m_softBodyWorldInfo.water_density>0)
1866         {
1867                 const btVector3 c=      btVector3((btScalar)0.25,(btScalar)0.25,1);
1868                 const btScalar  a=      (btScalar)0.5;
1869                 const btVector3 n=      m_softBodyWorldInfo.water_normal;
1870                 const btVector3 o=      -n*m_softBodyWorldInfo.water_offset;
1871                 const btVector3 x=      btCross(n,axis[n.minAxis()]).normalized();
1872                 const btVector3 y=      btCross(x,n).normalized();
1873                 const btScalar  s=      25;
1874                 idraw->drawTriangle(o-x*s-y*s,o+x*s-y*s,o+x*s+y*s,c,a);
1875                 idraw->drawTriangle(o-x*s-y*s,o+x*s+y*s,o-x*s+y*s,c,a);
1876         }
1877         //
1878
1879         int lineWidth=280;
1880         int xStart = m_glutScreenWidth - lineWidth;
1881         int yStart = 20;
1882
1883         if((getDebugMode() & btIDebugDraw::DBG_NoHelpText)==0)
1884         {
1885                 setOrthographicProjection();
1886                 glDisable(GL_LIGHTING);
1887                 glColor3f(0, 0, 0);
1888                 char buf[124];
1889
1890                 glRasterPos3f(xStart, yStart, 0);
1891                 if (sDemoMode)
1892                 {
1893                         sprintf(buf,"d to toggle demo mode (on)");
1894                 } else
1895                 {
1896                         sprintf(buf,"d to toggle demo mode (off)");
1897                 }
1898                 GLDebugDrawString(xStart,20,buf);
1899                 glRasterPos3f(xStart, yStart, 0);
1900                 sprintf(buf,"] for next demo (%d)",current_demo);
1901                 yStart+=20;
1902                 GLDebugDrawString(xStart,yStart,buf);
1903                 glRasterPos3f(xStart, yStart, 0);
1904                 sprintf(buf,"c to visualize clusters");
1905                 yStart+=20;
1906                 GLDebugDrawString(xStart,yStart,buf);
1907                 glRasterPos3f(xStart, yStart, 0);
1908                 sprintf(buf,"; to toggle camera mode");
1909                 yStart+=20;
1910                 GLDebugDrawString(xStart,yStart,buf);
1911                 glRasterPos3f(xStart, yStart, 0);
1912         sprintf(buf,"n,m,l,k for power and steering");
1913                 yStart+=20;
1914                 GLDebugDrawString(xStart,yStart,buf);
1915
1916
1917                 resetPerspectiveProjection();
1918                 glEnable(GL_LIGHTING);
1919         }
1920
1921         DemoApplication::renderme();
1922
1923 }
1924
1925 void    SoftDemo::setDrawClusters(bool drawClusters)
1926 {
1927         if (drawClusters)
1928         {
1929                 getSoftDynamicsWorld()->setDrawFlags(getSoftDynamicsWorld()->getDrawFlags()|fDrawFlags::Clusters);
1930         } else
1931         {
1932                 getSoftDynamicsWorld()->setDrawFlags(getSoftDynamicsWorld()->getDrawFlags()& (~fDrawFlags::Clusters));
1933         }
1934 }
1935
1936
1937
1938 void    SoftDemo::keyboardCallback(unsigned char key, int x, int y)
1939 {
1940         switch(key)
1941         {
1942         case    'd':    sDemoMode = !sDemoMode; break;
1943         case    'n':    motorcontrol.maxtorque=10;motorcontrol.goal+=1;break;
1944         case    'm':    motorcontrol.maxtorque=10;motorcontrol.goal-=1;break;
1945         case    'l':    steercontrol_f.angle+=0.1;steercontrol_r.angle+=0.1;break;
1946         case    'k':    steercontrol_f.angle-=0.1;steercontrol_r.angle-=0.1;break;
1947         case    ']':    ++current_demo;clientResetScene();break;
1948         case    '[':    --current_demo;clientResetScene();break;
1949         case    ',':    m_raycast=!m_raycast;break;
1950         case    ';':    m_autocam=!m_autocam;break;
1951         case    'c':    getSoftDynamicsWorld()->setDrawFlags(getSoftDynamicsWorld()->getDrawFlags()^fDrawFlags::Clusters);break;
1952         case    '`':
1953                 {
1954                         btSoftBodyArray&        sbs=getSoftDynamicsWorld()->getSoftBodyArray();
1955                         for(int ib=0;ib<sbs.size();++ib)
1956                         {
1957                                 btSoftBody*     psb=sbs[ib];
1958                                 psb->staticSolve(128);
1959                         }
1960                 }
1961                 break;
1962         default:                DemoApplication::keyboardCallback(key,x,y);
1963         }
1964 }
1965
1966 //
1967 void    SoftDemo::mouseMotionFunc(int x,int y)
1968 {
1969         if(m_node&&(m_results.fraction<1.f))
1970         {
1971                 if(!m_drag)
1972                 {
1973 #define SQ(_x_) (_x_)*(_x_)
1974                         if((SQ(x-m_lastmousepos[0])+SQ(y-m_lastmousepos[1]))>6)
1975                         {
1976                                 m_drag=true;
1977                         }
1978 #undef SQ
1979                 }
1980                 if(m_drag)
1981                 {
1982                         m_lastmousepos[0]       =       x;
1983                         m_lastmousepos[1]       =       y;
1984                 }
1985         }
1986         else
1987         {
1988                 DemoApplication::mouseMotionFunc(x,y);
1989         }
1990 }
1991
1992 //
1993 void    SoftDemo::mouseFunc(int button, int state, int x, int y)
1994 {
1995         if(button==0)
1996         {
1997                 switch(state)
1998                 {
1999                         case    0:
2000                         {
2001                                 m_results.fraction=1.f;
2002                                 DemoApplication::mouseFunc(button,state,x,y);
2003                                 if(!m_pickConstraint)
2004                                 {
2005                                         const btVector3                 rayFrom=m_cameraPosition;
2006                                         const btVector3                 rayTo=getRayTo(x,y);
2007                                         const btVector3                 rayDir=(rayTo-rayFrom).normalized();
2008                                         btSoftBodyArray&                sbs=getSoftDynamicsWorld()->getSoftBodyArray();
2009                                         for(int ib=0;ib<sbs.size();++ib)
2010                                         {
2011                                                 btSoftBody*                             psb=sbs[ib];
2012                                                 btSoftBody::sRayCast    res;
2013                                                 if(psb->rayTest(rayFrom,rayTo,res))
2014                                                 {
2015                                                         m_results=res;
2016                                                 }
2017                                         }
2018                                         if(m_results.fraction<1.f)
2019                                         {
2020                                                 m_impact                        =       rayFrom+(rayTo-rayFrom)*m_results.fraction;
2021                                                 m_drag                          =       m_cutting ? false : true;
2022                                                 m_lastmousepos[0]       =       x;
2023                                                 m_lastmousepos[1]       =       y;
2024                                                 m_node                          =       0;
2025                                                 switch(m_results.feature)
2026                                                 {
2027                                                 case btSoftBody::eFeature::Tetra:
2028                                                         {
2029                                                                 btSoftBody::Tetra&      tet=m_results.body->m_tetras[m_results.index];
2030                                                                 m_node=tet.m_n[0];
2031                                                                 for(int i=1;i<4;++i)
2032                                                                 {
2033                                                                         if(     (m_node->m_x-m_impact).length2()>
2034                                                                                 (tet.m_n[i]->m_x-m_impact).length2())
2035                                                                         {
2036                                                                                 m_node=tet.m_n[i];
2037                                                                         }
2038                                                                 }
2039                                                                 break;
2040                                                         }
2041                                                 case    btSoftBody::eFeature::Face:
2042                                                         {
2043                                                                 btSoftBody::Face&       f=m_results.body->m_faces[m_results.index];
2044                                                                 m_node=f.m_n[0];
2045                                                                 for(int i=1;i<3;++i)
2046                                                                 {
2047                                                                         if(     (m_node->m_x-m_impact).length2()>
2048                                                                                 (f.m_n[i]->m_x-m_impact).length2())
2049                                                                         {
2050                                                                                 m_node=f.m_n[i];
2051                                                                         }
2052                                                                 }
2053                                                         }
2054                                                         break;
2055                                                 }
2056                                                 if(m_node) m_goal=m_node->m_x;
2057                                                 return;
2058                                         }
2059                                 }
2060                         }
2061                         break;
2062                 case    1:
2063                         if((!m_drag)&&m_cutting&&(m_results.fraction<1.f))
2064                         {
2065                                 ImplicitSphere  isphere(m_impact,1);
2066                                 printf("Mass before: %f\r\n",m_results.body->getTotalMass());
2067                                 m_results.body->refine(&isphere,0.0001,true);
2068                                 printf("Mass after: %f\r\n",m_results.body->getTotalMass());
2069                         }
2070                         m_results.fraction=1.f;
2071                         m_drag=false;
2072                         DemoApplication::mouseFunc(button,state,x,y);
2073                         break;
2074                 }
2075         }
2076         else
2077         {
2078                 DemoApplication::mouseFunc(button,state,x,y);
2079         }
2080 }
2081
2082
2083 void    SoftDemo::initPhysics()
2084 {
2085         ///create concave ground mesh
2086
2087
2088         m_azi = 0;
2089
2090         //reset and disable motorcontrol at the start
2091         motorcontrol.goal = 0;
2092         motorcontrol.maxtorque = 0;
2093
2094         btCollisionShape* groundShape = 0;
2095         {
2096                 int i;
2097                 int j;
2098
2099                 const int NUM_VERTS_X = 30;
2100                 const int NUM_VERTS_Y = 30;
2101                 const int totalVerts = NUM_VERTS_X*NUM_VERTS_Y;
2102                 const int totalTriangles = 2*(NUM_VERTS_X-1)*(NUM_VERTS_Y-1);
2103
2104                 gGroundVertices = new btVector3[totalVerts];
2105                 gGroundIndices = new int[totalTriangles*3];
2106
2107                 btScalar offset(-50);
2108
2109                 for ( i=0;i<NUM_VERTS_X;i++)
2110                 {
2111                         for (j=0;j<NUM_VERTS_Y;j++)
2112                         {
2113                                 gGroundVertices[i+j*NUM_VERTS_X].setValue((i-NUM_VERTS_X*0.5f)*TRIANGLE_SIZE,
2114                                         //0.f,
2115                                         waveheight*sinf((float)i)*cosf((float)j+offset),
2116                                         (j-NUM_VERTS_Y*0.5f)*TRIANGLE_SIZE);
2117                         }
2118                 }
2119
2120                 int vertStride = sizeof(btVector3);
2121                 int indexStride = 3*sizeof(int);
2122
2123                 int index=0;
2124                 for ( i=0;i<NUM_VERTS_X-1;i++)
2125                 {
2126                         for (int j=0;j<NUM_VERTS_Y-1;j++)
2127                         {
2128                                 gGroundIndices[index++] = j*NUM_VERTS_X+i;
2129                                 gGroundIndices[index++] = j*NUM_VERTS_X+i+1;
2130                                 gGroundIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
2131
2132                                 gGroundIndices[index++] = j*NUM_VERTS_X+i;
2133                                 gGroundIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
2134                                 gGroundIndices[index++] = (j+1)*NUM_VERTS_X+i;
2135                         }
2136                 }
2137
2138                 btTriangleIndexVertexArray* indexVertexArrays = new btTriangleIndexVertexArray(totalTriangles,
2139                         gGroundIndices,
2140                         indexStride,
2141                         totalVerts,(btScalar*) &gGroundVertices[0].x(),vertStride);
2142
2143                 bool useQuantizedAabbCompression = true;
2144
2145                 groundShape = new btBvhTriangleMeshShape(indexVertexArrays,useQuantizedAabbCompression);
2146                 groundShape->setMargin(0.5);
2147         }
2148
2149         m_collisionShapes.push_back(groundShape);
2150
2151         btCollisionShape* groundBox = new btBoxShape (btVector3(100,CUBE_HALF_EXTENTS,100));
2152         m_collisionShapes.push_back(groundBox);
2153
2154         btCompoundShape* cylinderCompound = new btCompoundShape;
2155         btCollisionShape* cylinderShape = new btCylinderShape (btVector3(CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS,CUBE_HALF_EXTENTS));
2156         btTransform localTransform;
2157         localTransform.setIdentity();
2158         cylinderCompound->addChildShape(localTransform,cylinderShape);
2159         btQuaternion orn(btVector3(0,1,0),SIMD_PI);
2160         localTransform.setRotation(orn);
2161         cylinderCompound->addChildShape(localTransform,cylinderShape);
2162
2163         m_collisionShapes.push_back(cylinderCompound);
2164
2165
2166         m_dispatcher=0;
2167
2168         ///register some softbody collision algorithms on top of the default btDefaultCollisionConfiguration
2169         m_collisionConfiguration = new btSoftBodyRigidBodyCollisionConfiguration();
2170
2171
2172         m_dispatcher = new      btCollisionDispatcher(m_collisionConfiguration);
2173         m_softBodyWorldInfo.m_dispatcher = m_dispatcher;
2174
2175         ////////////////////////////
2176         ///Register softbody versus softbody collision algorithm
2177
2178
2179         ///Register softbody versus rigidbody collision algorithm
2180
2181
2182         ////////////////////////////
2183
2184         btVector3 worldAabbMin(-1000,-1000,-1000);
2185         btVector3 worldAabbMax(1000,1000,1000);
2186
2187         m_broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax,maxProxies);
2188
2189         m_softBodyWorldInfo.m_broadphase = m_broadphase;
2190
2191         btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver();
2192
2193         m_solver = solver;
2194
2195         btSoftBodySolver* softBodySolver = 0;
2196 #ifdef USE_AMD_OPENCL
2197
2198         static bool once = true;
2199         if (once)
2200         {
2201                 once=false;
2202                 initCL(0,0);
2203         }
2204
2205         if( g_openCLSIMDSolver  )
2206                 delete g_openCLSIMDSolver;
2207         if( g_softBodyOutput )
2208                 delete g_softBodyOutput;
2209
2210         if (1)
2211         {
2212                 g_openCLSIMDSolver = new btOpenCLSoftBodySolverSIMDAware( g_cqCommandQue, g_cxMainContext);
2213         //      g_openCLSIMDSolver = new btOpenCLSoftBodySolver( g_cqCommandQue, g_cxMainContext);
2214                 g_openCLSIMDSolver->setCLFunctions(new CachingCLFunctions(g_cqCommandQue, g_cxMainContext));
2215         }
2216
2217
2218
2219         softBodySolver = g_openCLSIMDSolver;
2220         g_softBodyOutput = new btSoftBodySolverOutputCLtoCPU;
2221 #endif //USE_AMD_OPENCL
2222
2223         btDiscreteDynamicsWorld* world = new btSoftRigidDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration,softBodySolver);
2224         m_dynamicsWorld = world;
2225         m_dynamicsWorld->setInternalTickCallback(pickingPreTickCallback,this,true);
2226
2227
2228         m_dynamicsWorld->getDispatchInfo().m_enableSPU = true;
2229         m_dynamicsWorld->setGravity(btVector3(0,-10,0));
2230         m_softBodyWorldInfo.m_gravity.setValue(0,-10,0);
2231
2232         //      clientResetScene();
2233
2234         m_softBodyWorldInfo.m_sparsesdf.Initialize();
2235         clientResetScene();
2236 }
2237
2238
2239
2240
2241
2242
2243 void    SoftDemo::exitPhysics()
2244 {
2245
2246         //cleanup in the reverse order of creation/initialization
2247
2248         //remove the rigidbodies from the dynamics world and delete them
2249         int i;
2250         for (i=m_dynamicsWorld->getNumCollisionObjects()-1; i>=0 ;i--)
2251         {
2252                 btCollisionObject* obj = m_dynamicsWorld->getCollisionObjectArray()[i];
2253                 btRigidBody* body = btRigidBody::upcast(obj);
2254                 if (body && body->getMotionState())
2255                 {
2256                         delete body->getMotionState();
2257                 }
2258                 m_dynamicsWorld->removeCollisionObject( obj );
2259                 delete obj;
2260         }
2261
2262         //delete collision shapes
2263         for (int j=0;j<m_collisionShapes.size();j++)
2264         {
2265                 btCollisionShape* shape = m_collisionShapes[j];
2266                 m_collisionShapes[j] = 0;
2267                 delete shape;
2268         }
2269
2270         //delete dynamics world
2271         delete m_dynamicsWorld;
2272
2273         //delete solver
2274         delete m_solver;
2275
2276         //delete broadphase
2277         delete m_broadphase;
2278
2279         //delete dispatcher
2280         delete m_dispatcher;
2281
2282
2283
2284         delete m_collisionConfiguration;
2285
2286
2287 }
2288
2289
2290
2291
2292
2293