virtual void renderScene() { if (m_app && m_app->m_renderer) { m_app->m_renderer->renderScene(); m_app->m_renderer->clearZBuffer(); m_app->drawText3D("X",1,0,0,1); m_app->drawText3D("Y",0,1,0,1); m_app->drawText3D("Z",0,0,1,1); for (int i=0;i<gTotalPoints;i++) { const lwContactPoint& contact = pointsOut[i]; btVector3 color(1,1,0); btScalar lineWidth=3; if (contact.m_distance<0) { color.setValue(1,0,0); } m_app->m_renderer->drawLine(contact.m_ptOnAWorld,contact.m_ptOnBWorld,color,lineWidth); } } }
virtual void initPhysics() { ///create some graphics proxy for the tracking target ///the endeffector tries to track it using Inverse Kinematics { int sphereId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_MEDIUM); b3Quaternion orn(0, 0, 0, 1); b3Vector4 color = b3MakeVector4(1., 0.3, 0.3, 1); b3Vector3 scaling = b3MakeVector3(.02, .02, .02); m_targetSphereInstance = m_app->m_renderer->registerGraphicsInstance(sphereId, m_targetPos, orn, color, scaling); } m_app->m_renderer->writeTransforms(); int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER; m_robotSim.setGuiHelper(m_guiHelper); bool connected = m_robotSim.connect(mode); // 0;//m_robotSim.connect(m_guiHelper); b3Printf("robotSim connected = %d",connected); { m_kukaIndex = m_robotSim.loadURDF("kuka_iiwa/model.urdf"); if (m_kukaIndex >=0) { int numJoints = m_robotSim.getNumJoints(m_kukaIndex); b3Printf("numJoints = %d",numJoints); for (int i=0;i<numJoints;i++) { b3JointInfo jointInfo; m_robotSim.getJointInfo(m_kukaIndex,i,&jointInfo); b3Printf("joint[%d].m_jointName=%s",i,jointInfo.m_jointName); } /* int wheelJointIndices[4]={2,3,6,7}; int wheelTargetVelocities[4]={-10,-10,-10,-10}; for (int i=0;i<4;i++) { b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); controlArgs.m_targetVelocity = wheelTargetVelocities[i]; controlArgs.m_maxTorqueValue = 1e30; m_robotSim.setJointMotorControl(m_kukaIndex,wheelJointIndices[i],controlArgs); } */ } { m_robotSim.loadURDF("plane.urdf"); m_robotSim.setGravity(b3MakeVector3(0,0,0)); } } }
GripperGraspExample(GUIHelperInterface* helper, int options) :m_app(helper->getAppInterface()), m_guiHelper(helper), m_options(options), m_gripperIndex(-1) { m_app->setUpAxis(2); }
virtual void renderScene() { m_app->m_renderer->renderScene(); m_app->drawText3D("X",1,0,0,1); m_app->drawText3D("Y",0,1,0,1); m_app->drawText3D("Z",0,0,1,1); for (int i=0;i<m_contactPoints.size();i++) { const LWContactPoint& contact = m_contactPoints[i]; b3Vector3 color=b3MakeVector3(1,1,0); float lineWidth=3; if (contact.m_distance<0) { color.setValue(1,0,0); } m_app->m_renderer->drawLine(contact.m_ptOnAWorld,contact.m_ptOnBWorld,color,lineWidth); } }
MultiThreadingExample(GUIHelperInterface* guiHelper, int tutorialIndex) :m_app(guiHelper->getAppInterface()), m_threadSupport(0), m_numThreads(8) { //int numBodies = 1; m_app->setUpAxis(1); }
RenderInstancingDemo(CommonGraphicsApp* app) :m_app(app), m_x(0), m_y(0), m_z(0) { m_app->setUpAxis(2); { b3Vector3 extents=b3MakeVector3(100,100,100); extents[m_app->getUpAxis()]=1; int xres = 20; int yres = 20; b3Vector4 color0=b3MakeVector4(0.1, 0.1, 0.1,1); b3Vector4 color1=b3MakeVector4(0.6, 0.6, 0.6,1); m_app->registerGrid(xres, yres, color0, color1); } { int boxId = m_app->registerCubeShape(0.1,0.1,0.1); for (int i=-numCubesX/2;i<numCubesX/2;i++) { for (int j = -numCubesY/2;j<numCubesY/2;j++) { b3Vector3 pos=b3MakeVector3(i,j,j); pos[app->getUpAxis()] = 1; b3Quaternion orn(0,0,0,1); b3Vector4 color=b3MakeVector4(0.3,0.3,0.3,1); b3Vector3 scaling=b3MakeVector3(1,1,1); int instanceId = m_app->m_renderer->registerGraphicsInstance(boxId,pos,orn,color,scaling); m_movingInstances.push_back(instanceId); } } } m_app->m_renderer->writeTransforms(); }
KukaGraspExample(GUIHelperInterface* helper, int /* options */) :m_app(helper->getAppInterface()), m_guiHelper(helper), // m_options(options), m_kukaIndex(-1), m_time(0) { m_targetPos.setValue(0.5,0,1); m_worldPos.setValue(0, 0, 0); m_app->setUpAxis(2); }
virtual void stepSimulation(float deltaTime) { m_x+=0.01f; m_y+=0.01f; m_z+=0.01f; int index=0; for (int i=-numCubesX/2;i<numCubesX/2;i++) { for (int j = -numCubesY/2;j<numCubesY/2;j++) { b3Vector3 pos=b3MakeVector3(i,j,j); pos[m_app->getUpAxis()] = 1+1*b3Sin(m_x+i-j); float orn[4]={0,0,0,1}; m_app->m_renderer->writeSingleInstanceTransformToCPU(pos,orn,m_movingInstances[index++]); } } m_app->m_renderer->writeTransforms(); }
Tutorial(GUIHelperInterface* guiHelper, int tutorialIndex) :m_app(guiHelper->getAppInterface()), m_guiHelper(guiHelper), m_tutorialIndex(tutorialIndex), m_stage(0), m_counter(0), m_timeSeriesCanvas0(0), m_timeSeriesCanvas1(0) { int numBodies = 1; m_app->setUpAxis(1); m_app->m_renderer->enableBlend(true); switch (m_tutorialIndex) { case TUT_VELOCITY: { numBodies=10; m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Constant Velocity"); m_timeSeriesCanvas0 ->setupTimeSeries(2,60, 0); m_timeSeriesCanvas0->addDataSource("X position (m)", 255,0,0); m_timeSeriesCanvas0->addDataSource("X velocity (m/s)", 0,0,255); m_timeSeriesCanvas0->addDataSource("dX/dt (m/s)", 0,0,0); break; } case TUT_ACCELERATION: { numBodies=10; m_timeSeriesCanvas1 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,256,512,"Constant Acceleration"); m_timeSeriesCanvas1 ->setupTimeSeries(50,60, 0); m_timeSeriesCanvas1->addDataSource("Y position (m)", 255,0,0); m_timeSeriesCanvas1->addDataSource("Y velocity (m/s)", 0,0,255); m_timeSeriesCanvas1->addDataSource("dY/dt (m/s)", 0,0,0); break; } case TUT_COLLISION: { numBodies=2; m_timeSeriesCanvas1 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,200,"Distance"); m_timeSeriesCanvas1 ->setupTimeSeries(1.5,60, 0); m_timeSeriesCanvas1->addDataSource("distance", 255,0,0); break; } case TUT_SOLVE_CONTACT_CONSTRAINT: { numBodies=2; m_timeSeriesCanvas1 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,200,"Collision Impulse"); m_timeSeriesCanvas1 ->setupTimeSeries(1.5,60, 0); m_timeSeriesCanvas1->addDataSource("Distance", 0,0,255); m_timeSeriesCanvas1->addDataSource("Impulse magnutide", 255,0,0); { SliderParams slider("Restitution",&gRestitution); slider.m_minVal=0; slider.m_maxVal=1; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("Mass A",&gMassA); slider.m_minVal=0; slider.m_maxVal=100; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } { SliderParams slider("Mass B",&gMassB); slider.m_minVal=0; slider.m_maxVal=100; m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider); } break; } default: { m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Unknown"); m_timeSeriesCanvas0 ->setupTimeSeries(1,60, 0); } }; if (m_tutorialIndex==TUT_VELOCITY) { int boxId = m_app->registerCubeShape(100,1,100); b3Vector3 pos = b3MakeVector3(0,-3.5,0); b3Quaternion orn(0,0,0,1); b3Vector4 color = b3MakeVector4(1,1,1,1); b3Vector3 scaling = b3MakeVector3(1,1,1); m_app->m_renderer->registerGraphicsInstance(boxId,pos,orn,color,scaling); } for (int i=0;i<numBodies;i++) { m_bodies.push_back(new LWRigidBody()); } for (int i=0;i<m_bodies.size();i++) { m_bodies[i]->m_worldPose.m_position.setValue((i/4)*5,3,(i&3)*5); } { int textureIndex = -1; if (1) { int width,height,n; const char* filename = "data/cube.png"; const unsigned char* image=0; const char* prefix[]={"./","../","../../","../../../","../../../../"}; int numprefix = sizeof(prefix)/sizeof(const char*); for (int i=0;!image && i<numprefix;i++) { char relativeFileName[1024]; sprintf(relativeFileName,"%s%s",prefix[i],filename); image = stbi_load(relativeFileName, &width, &height, &n, 0); } b3Assert(image); if (image) { textureIndex = m_app->m_renderer->registerTexture(image,width,height); } } // int boxId = m_app->registerCubeShape(1,1,1,textureIndex); int boxId = m_app->registerGraphicsUnitSphereShape(SPHERE_LOD_HIGH, textureIndex); b3Vector4 color = b3MakeVector4(1,1,1,0.8); b3Vector3 scaling = b3MakeVector3(SPHERE_RADIUS,SPHERE_RADIUS,SPHERE_RADIUS); for (int i=0;i<m_bodies.size();i++) { m_bodies[i]->m_collisionShape.m_sphere.m_radius = SPHERE_RADIUS; m_bodies[i]->m_collisionShape.m_type = LW_SPHERE_TYPE; m_bodies[i]->m_graphicsIndex = m_app->m_renderer->registerGraphicsInstance(boxId,m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation,color,scaling); m_app->m_renderer->writeSingleInstanceTransformToCPU(m_bodies[i]->m_worldPose.m_position, m_bodies[i]->m_worldPose.m_orientation, m_bodies[i]->m_graphicsIndex); } } if (m_tutorialIndex == TUT_SOLVE_CONTACT_CONSTRAINT) { m_bodies[0]->m_invMass = gMassA? 1./gMassA : 0; m_bodies[0]->m_collisionShape.m_sphere.computeLocalInertia(gMassA,m_bodies[0]->m_localInertia); m_bodies[1]->m_invMass =gMassB? 1./gMassB : 0; m_bodies[1]->m_collisionShape.m_sphere.computeLocalInertia(gMassB,m_bodies[1]->m_localInertia); if (gMassA) m_bodies[0]->m_linearVelocity.setValue(0,0,1); if (gMassB) m_bodies[1]->m_linearVelocity.setValue(0,0,-1); } m_app->m_renderer->writeTransforms(); }
CollisionTutorialBullet2(GUIHelperInterface* guiHelper, int tutorialIndex) :m_app(guiHelper->getAppInterface()), m_guiHelper(guiHelper), m_tutorialIndex(tutorialIndex), m_collisionSdkHandle(0), m_collisionWorldHandle(0), m_stage(0), m_counter(0), m_timeSeriesCanvas0(0) { gTotalPoints = 0; m_app->setUpAxis(1); m_app->m_renderer->enableBlend(true); switch (m_tutorialIndex) { case TUT_SPHERE_PLANE_RTB3: case TUT_SPHERE_PLANE_BULLET2: { if (m_tutorialIndex==TUT_SPHERE_PLANE_BULLET2) { m_collisionSdkHandle = plCreateBullet2CollisionSdk(); } else { #ifndef DISABLE_REAL_TIME_BULLET3_COLLISION_SDK m_collisionSdkHandle = plCreateRealTimeBullet3CollisionSdk(); #endif //DISABLE_REAL_TIME_BULLET3_COLLISION_SDK } if (m_collisionSdkHandle) { int maxNumObjsCapacity=1024; int maxNumShapesCapacity=1024; int maxNumPairsCapacity=16384; btAlignedObjectArray<plCollisionObjectHandle> colliders; m_collisionWorldHandle = plCreateCollisionWorld(m_collisionSdkHandle,maxNumObjsCapacity,maxNumShapesCapacity,maxNumPairsCapacity); //create objects, do query etc { float radius = 1.f; void* userPointer = 0; { for (int j=0;j<sNumCompounds;j++) { plCollisionShapeHandle compoundShape = plCreateCompoundShape(m_collisionSdkHandle,m_collisionWorldHandle); for (int i=0;i<sNumSpheres;i++) { btVector3 childPos(i*1.5,0,0); btQuaternion childOrn(0,0,0,1); btVector3 scaling(radius,radius,radius); plCollisionShapeHandle childShape = plCreateSphereShape(m_collisionSdkHandle, m_collisionWorldHandle,radius); plAddChildShape(m_collisionSdkHandle,m_collisionWorldHandle,compoundShape, childShape,childPos,childOrn); //m_guiHelper->createCollisionObjectGraphicsObject(colObj,color); } if (m_tutorialIndex==TUT_SPHERE_PLANE_BULLET2) { btCollisionShape* colShape = (btCollisionShape*) compoundShape; m_guiHelper->createCollisionShapeGraphicsObject(colShape); } else { } { btVector3 pos(j*sNumSpheres*1.5,-2.4,0); btQuaternion orn(0,0,0,1); plCollisionObjectHandle colObjHandle = plCreateCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,userPointer, -1,compoundShape,pos,orn); if (m_tutorialIndex==TUT_SPHERE_PLANE_BULLET2) { btCollisionObject* colObj = (btCollisionObject*) colObjHandle; btVector4 color=sColors[j&3]; m_guiHelper->createCollisionObjectGraphicsObject(colObj,color); colliders.push_back(colObjHandle); plAddCollisionObject(m_collisionSdkHandle, m_collisionWorldHandle,colObjHandle); } } } } } { plCollisionShapeHandle colShape = plCreatePlaneShape(m_collisionSdkHandle, m_collisionWorldHandle,0,1,0,-3.5); btVector3 pos(0,0,0); btQuaternion orn(0,0,0,1); void* userPointer = 0; plCollisionObjectHandle colObj = plCreateCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,userPointer, 0,colShape,pos,orn); colliders.push_back(colObj); plAddCollisionObject(m_collisionSdkHandle, m_collisionWorldHandle,colObj); } int numContacts = plCollide(m_collisionSdkHandle,m_collisionWorldHandle,colliders[0],colliders[1],pointsOut,sPointCapacity); printf("numContacts = %d\n", numContacts); void* myUserPtr = 0; plWorldCollide(m_collisionSdkHandle,m_collisionWorldHandle,myNearCallback, myUserPtr); printf("total points=%d\n",gTotalPoints); //plRemoveCollisionObject(m_collisionSdkHandle,m_collisionWorldHandle,colObj); //plDeleteCollisionObject(m_collisionSdkHandle,colObj); //plDeleteShape(m_collisionSdkHandle,colShape); } /* m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Constant Velocity"); m_timeSeriesCanvas0 ->setupTimeSeries(2,60, 0); m_timeSeriesCanvas0->addDataSource("X position (m)", 255,0,0); m_timeSeriesCanvas0->addDataSource("X velocity (m/s)", 0,0,255); m_timeSeriesCanvas0->addDataSource("dX/dt (m/s)", 0,0,0); */ break; } default: { m_timeSeriesCanvas0 = new TimeSeriesCanvas(m_app->m_2dCanvasInterface,512,256,"Unknown"); m_timeSeriesCanvas0 ->setupTimeSeries(1,60, 0); } }; { int boxId = m_app->registerCubeShape(100,0.01,100); b3Vector3 pos = b3MakeVector3(0,-3.5,0); b3Quaternion orn(0,0,0,1); b3Vector4 color = b3MakeVector4(1,1,1,1); b3Vector3 scaling = b3MakeVector3(1,1,1); m_app->m_renderer->registerGraphicsInstance(boxId,pos,orn,color,scaling); } { int textureIndex = -1; if (1) { int width,height,n; const char* filename = "data/cube.png"; const unsigned char* image=0; const char* prefix[]={"./","../","../../","../../../","../../../../"}; int numprefix = sizeof(prefix)/sizeof(const char*); for (int i=0;!image && i<numprefix;i++) { char relativeFileName[1024]; sprintf(relativeFileName,"%s%s",prefix[i],filename); image = stbi_load(relativeFileName, &width, &height, &n, 3); } b3Assert(image); if (image) { textureIndex = m_app->m_renderer->registerTexture(image,width,height); } } } m_app->m_renderer->writeTransforms(); }