int b3CpuRigidBodyPipeline::registerPhysicsInstance(float mass, const float* position, const float* orientation, int collidableIndex, int userData) { b3RigidBodyData body; int bodyIndex = m_data->m_rigidBodies.size(); body.m_invMass = mass ? 1.f/mass : 0.f; body.m_angVel.setValue(0,0,0); body.m_collidableIdx = collidableIndex; body.m_frictionCoeff = 0.3f; body.m_linVel.setValue(0,0,0); body.m_pos.setValue(position[0],position[1],position[2]); body.m_quat.setValue(orientation[0],orientation[1],orientation[2],orientation[3]); body.m_restituitionCoeff = 0.f; m_data->m_rigidBodies.push_back(body); if (collidableIndex>=0) { b3Aabb& worldAabb = m_data->m_aabbWorldSpace.expand(); b3Aabb localAabb = m_data->m_np->getLocalSpaceAabb(collidableIndex); b3Vector3 localAabbMin=b3MakeVector3(localAabb.m_min[0],localAabb.m_min[1],localAabb.m_min[2]); b3Vector3 localAabbMax=b3MakeVector3(localAabb.m_max[0],localAabb.m_max[1],localAabb.m_max[2]); b3Scalar margin = 0.01f; b3Transform t; t.setIdentity(); t.setOrigin(b3MakeVector3(position[0],position[1],position[2])); t.setRotation(b3Quaternion(orientation[0],orientation[1],orientation[2],orientation[3])); b3TransformAabb(localAabbMin,localAabbMax, margin,t,worldAabb.m_minVec,worldAabb.m_maxVec); m_data->m_bp->createProxy(worldAabb.m_minVec,worldAabb.m_maxVec,bodyIndex,0,1,1); // b3Vector3 aabbMin,aabbMax; // m_data->m_bp->getAabb(bodyIndex,aabbMin,aabbMax); } else { b3Error("registerPhysicsInstance using invalid collidableIndex\n"); } return bodyIndex; }
int main(int argc, char* argv[]) { b3CommandLineArgs args(argc, argv); std::string port = "COM9"; args.GetCmdLineArgument("port", port); int baud = 115200; args.GetCmdLineArgument("speed", baud); std::string mode = "SHARED_MEMORY"; args.GetCmdLineArgument("mode", mode); int disableGui = 0; args.GetCmdLineArgument("disableGui", disableGui); int disableShadows = 0; args.GetCmdLineArgument("disableShadows", disableShadows); int useKitchen = 0; args.GetCmdLineArgument("useKitchen", useKitchen); int deviceTypeFilter = VR_DEVICE_GENERIC_TRACKER; args.GetCmdLineArgument("deviceTypeFilter", deviceTypeFilter); printf("port=%s, speed=%d, connection mode=%s\n", port.c_str(), baud, mode.c_str()); b3RobotSimulatorClientAPI* sim = new b3RobotSimulatorClientAPI(); //Can also use eCONNECT_UDP,eCONNECT_TCP, for example: sim->connect(eCONNECT_UDP, "localhost", 1234); if (mode == "GUI") { sim->connect(eCONNECT_GUI); } else { if (mode == "DIRECT") { sim->connect(eCONNECT_DIRECT); } else { sim->connect(eCONNECT_SHARED_MEMORY); } } sim->setRealTimeSimulation(true); sim->setInternalSimFlags(0); sim->resetSimulation(); if (disableGui) { sim->configureDebugVisualizer(COV_ENABLE_GUI, 0); } if (disableShadows) { sim->configureDebugVisualizer(COV_ENABLE_SHADOWS, 0); } sim->setTimeOut(12345); //syncBodies is only needed when connecting to an existing physics server that has already some bodies sim->syncBodies(); b3Scalar fixedTimeStep = 1. / 240.; sim->setTimeStep(fixedTimeStep); b3Quaternion q = sim->getQuaternionFromEuler(b3MakeVector3(0.1, 0.2, 0.3)); b3Vector3 rpy; rpy = sim->getEulerFromQuaternion(q); sim->setGravity(b3MakeVector3(0, 0, -9.8)); sim->setContactBreakingThreshold(0.001); if (useKitchen) { b3RobotSimulatorLoadFileResults res; sim->loadSDF("kitchens/1.sdf", res); } else { sim->loadURDF("plane_with_collision_audio.urdf"); } int handUid = -1; b3RobotSimulatorLoadFileResults mjcfResults; const char* mjcfFileName = "MPL/mpl2.xml"; if (sim->loadMJCF(mjcfFileName, mjcfResults)) { printf("mjcfResults = %d\n", mjcfResults.m_uniqueObjectIds.size()); if (mjcfResults.m_uniqueObjectIds.size() == 1) { handUid = mjcfResults.m_uniqueObjectIds[0]; } } if (handUid < 0) { printf("Cannot load MJCF file %s\n", mjcfFileName); } #ifdef TOUCH b3Vector3 handPos = b3MakeVector3(-0.10, -0.03, 0.02); b3Vector3 rollPitchYaw = b3MakeVector3(0.5 * B3_PI, 0, 1.25 * B3_PI); //-B3_PI/2,0,B3_PI/2); handOrn = sim->getQuaternionFromEuler(rollPitchYaw); #else b3Quaternion handOrn = sim->getQuaternionFromEuler(b3MakeVector3(3.14, -3.14 / 2, 0)); b3Vector3 handPos = b3MakeVector3(-0.05, 0, 0.02); #endif b3Vector3 handStartPosWorld = b3MakeVector3(0.500000, 0.300006, 0.900000); b3Quaternion handStartOrnWorld = b3Quaternion ::getIdentity(); b3JointInfo jointInfo; jointInfo.m_jointType = eFixedType; printf("handStartOrnWorld=%f,%f,%f,%f\n", handStartOrnWorld[0], handStartOrnWorld[1], handStartOrnWorld[2], handStartOrnWorld[3]); jointInfo.m_childFrame[0] = handStartPosWorld[0]; jointInfo.m_childFrame[1] = handStartPosWorld[1]; jointInfo.m_childFrame[2] = handStartPosWorld[2]; jointInfo.m_childFrame[3] = handStartOrnWorld[0]; jointInfo.m_childFrame[4] = handStartOrnWorld[1]; jointInfo.m_childFrame[5] = handStartOrnWorld[2]; jointInfo.m_childFrame[6] = handStartOrnWorld[3]; jointInfo.m_parentFrame[0] = handPos[0]; jointInfo.m_parentFrame[1] = handPos[1]; jointInfo.m_parentFrame[2] = handPos[2]; jointInfo.m_parentFrame[3] = handOrn[0]; jointInfo.m_parentFrame[4] = handOrn[1]; jointInfo.m_parentFrame[5] = handOrn[2]; jointInfo.m_parentFrame[6] = handOrn[3]; sim->resetBasePositionAndOrientation(handUid, handStartPosWorld, handStartOrnWorld); int handConstraintId = sim->createConstraint(handUid, -1, -1, -1, &jointInfo); double maxFingerForce = 10; double maxArmForce = 1000; for (int j = 0; j < sim->getNumJoints(handUid); j++) { b3RobotSimulatorJointMotorArgs controlArgs(CONTROL_MODE_POSITION_VELOCITY_PD); controlArgs.m_maxTorqueValue = maxFingerForce; controlArgs.m_kp = 0.1; controlArgs.m_kd = 1; controlArgs.m_targetPosition = 0; controlArgs.m_targetVelocity = 0; sim->setJointMotorControl(handUid, j, controlArgs); } sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.300000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107))); sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.200000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107))); sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.100000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107))); sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.000000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107))); sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.900000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107))); sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.800000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107))); sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.700000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107))); sim->loadURDF("jenga/jenga.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.600000, -0.700000, 0.750000), b3Quaternion(0.000000, 0.707107, 0.000000, 0.707107))); sim->loadURDF("table/table.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(1.000000, -0.200000, 0.000000), b3Quaternion(0.000000, 0.000000, 0.707107, 0.707107))); sim->loadURDF("cube_small.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.950000, -0.100000, 0.700000), b3Quaternion(0.000000, 0.000000, 0.707107, 0.707107))); sim->loadURDF("sphere_small.urdf", b3RobotSimulatorLoadUrdfFileArgs(b3MakeVector3(0.850000, -0.400000, 0.700000), b3Quaternion(0.000000, 0.000000, 0.707107, 0.707107))); b3Clock clock; double startTime = clock.getTimeInSeconds(); double simWallClockSeconds = 20.; int vidLogId = -1; int minitaurLogId = -1; int rotateCamera = 0; serial::Serial my_serial; try { // port, baudrate, timeout in milliseconds my_serial.setBaudrate(baud); my_serial.setPort(port); my_serial.setBytesize(serial::sevenbits); my_serial.setParity(serial::parity_odd); my_serial.setStopbits(serial::stopbits_two); my_serial.setTimeout(serial::Timeout::simpleTimeout(0.01)); my_serial.open(); } catch (...) { printf("Cannot open port, use --port=PORTNAME\n"); exit(0); } if (!my_serial.isOpen()) { printf("Cannot open serial port\n"); return -1; } my_serial.flush(); while (sim->canSubmitCommand()) { clock.usleep(1); b3VREventsData vrEvents; sim->getVREvents(&vrEvents, deviceTypeFilter); //instead of iterating over all vr events, we just take the most up-to-date one if (vrEvents.m_numControllerEvents) { int i = vrEvents.m_numControllerEvents - 1; b3VRControllerEvent& e = vrEvents.m_controllerEvents[i]; // printf("e.pos=%f,%f,%f\n",e.m_pos[0],e.m_pos[1],e.m_pos[2]); b3JointInfo changeConstraintInfo; changeConstraintInfo.m_flags = 0; changeConstraintInfo.m_jointMaxForce = maxArmForce; changeConstraintInfo.m_flags |= eJointChangeMaxForce; changeConstraintInfo.m_childFrame[0] = e.m_pos[0]; changeConstraintInfo.m_childFrame[1] = e.m_pos[1]; changeConstraintInfo.m_childFrame[2] = e.m_pos[2]; changeConstraintInfo.m_flags |= eJointChangeChildFramePosition; changeConstraintInfo.m_childFrame[3] = e.m_orn[0]; changeConstraintInfo.m_childFrame[4] = e.m_orn[1]; changeConstraintInfo.m_childFrame[5] = e.m_orn[2]; changeConstraintInfo.m_childFrame[6] = e.m_orn[3]; changeConstraintInfo.m_flags |= eJointChangeChildFrameOrientation; sim->changeConstraint(handConstraintId, &changeConstraintInfo); } //read the serial output from the hand, extract into parts std::string result; try { result = my_serial.readline(); } catch (...) { } if (result.length()) { my_serial.flush(); int res = result.find("\n"); while (res < 0) { result += my_serial.readline(); res = result.find("\n"); } btAlignedObjectArray<std::string> pieces; btAlignedObjectArray<std::string> separators; separators.push_back(","); urdfStringSplit(pieces, result, separators); //printf("serial: %s\n", result.c_str()); if (pieces.size() == 6) { double pinkTarget = 0; double middleTarget = 0; double indexTarget = 0; double thumbTarget = 0; { int pink = atoi(pieces[1].c_str()); int middle = atoi(pieces[2].c_str()); int index = atoi(pieces[3].c_str()); int thumb = atoi(pieces[4].c_str()); pinkTarget = convertSensor(pink, 250, 400); middleTarget = convertSensor(middle, 250, 400); indexTarget = convertSensor(index, 250, 400); thumbTarget = convertSensor(thumb, 250, 400); } //printf("pink = %d, middle=%d, index=%d, thumb=%d\n", pink,middle,index,thumb); setJointMotorPositionControl(sim, handUid, 5, 1.3); setJointMotorPositionControl(sim, handUid, 7, thumbTarget); setJointMotorPositionControl(sim, handUid, 9, thumbTarget); setJointMotorPositionControl(sim, handUid, 11, thumbTarget); setJointMotorPositionControl(sim, handUid, 15, indexTarget); setJointMotorPositionControl(sim, handUid, 17, indexTarget); setJointMotorPositionControl(sim, handUid, 19, indexTarget); setJointMotorPositionControl(sim, handUid, 22, middleTarget); setJointMotorPositionControl(sim, handUid, 24, middleTarget); setJointMotorPositionControl(sim, handUid, 27, middleTarget); double ringTarget = 0.5f * (pinkTarget + middleTarget); setJointMotorPositionControl(sim, handUid, 30, ringTarget); setJointMotorPositionControl(sim, handUid, 32, ringTarget); setJointMotorPositionControl(sim, handUid, 34, ringTarget); setJointMotorPositionControl(sim, handUid, 38, pinkTarget); setJointMotorPositionControl(sim, handUid, 40, pinkTarget); setJointMotorPositionControl(sim, handUid, 42, pinkTarget); } } b3KeyboardEventsData keyEvents; sim->getKeyboardEvents(&keyEvents); //sim->stepSimulation(); if (rotateCamera) { static double yaw = 0; double distance = 1; yaw += 0.1; b3Vector3 basePos; b3Quaternion baseOrn; // sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn); // sim->resetDebugVisualizerCamera(distance,yaw,20,basePos); } //b3Clock::usleep(1000.*1000.*fixedTimeStep); } printf("serial.close()\n"); my_serial.close(); printf("sim->disconnect\n"); sim->disconnect(); printf("delete sim\n"); delete sim; printf("exit\n"); }
void PhysicsServerExample::renderScene() { B3_PROFILE("PhysicsServerExample::RenderScene"); static char line0[1024]; static char line1[1024]; if (gEnableRealTimeSimVR) { static int frameCount=0; static btScalar prevTime = m_clock.getTimeSeconds(); frameCount++; static btScalar worseFps = 1000000; int numFrames = 200; static int count = 0; count++; if (0 == (count & 1)) { btScalar curTime = m_clock.getTimeSeconds(); btScalar fps = 1. / (curTime - prevTime); prevTime = curTime; if (fps < worseFps) { worseFps = fps; } if (count > numFrames) { count = 0; sprintf(line0, "fps:%f frame:%d", worseFps, frameCount / 2); sprintf(line1, "drop:%d tscale:%f dt:%f, substep %f)", gDroppedSimulationSteps, simTimeScalingFactor,gDtInSec, gSubStep); gDroppedSimulationSteps = 0; worseFps = 1000000; } } #ifdef BT_ENABLE_VR if ((gInternalSimFlags&2 ) && m_tinyVrGui==0) { ComboBoxParams comboParams; comboParams.m_comboboxId = 0; comboParams.m_numItems = 0; comboParams.m_startItem = 0; comboParams.m_callback = 0;//MyComboBoxCallback; comboParams.m_userPointer = 0;//this; m_tinyVrGui = new TinyVRGui(comboParams,this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()); m_tinyVrGui->init(); } if (m_tinyVrGui) { b3Transform tr;tr.setIdentity(); tr.setOrigin(b3MakeVector3(gVRController2Pos[0],gVRController2Pos[1],gVRController2Pos[2])); tr.setRotation(b3Quaternion(gVRController2Orn[0],gVRController2Orn[1],gVRController2Orn[2],gVRController2Orn[3])); tr = tr*b3Transform(b3Quaternion(0,0,-SIMD_HALF_PI),b3MakeVector3(0,0,0)); b3Scalar dt = 0.01; m_tinyVrGui->clearTextArea(); m_tinyVrGui->grapicalPrintf(line0,0,0,0,0,0,255); m_tinyVrGui->grapicalPrintf(line1,0,16,255,255,255,255); m_tinyVrGui->tick(dt,tr); } #endif//BT_ENABLE_VR } ///debug rendering //m_args[0].m_cs->lock(); //gVRTeleportPos[0] += 0.01; vrOffset[12]=-gVRTeleportPos[0]; vrOffset[13]=-gVRTeleportPos[1]; vrOffset[14]=-gVRTeleportPos[2]; this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()-> getActiveCamera()->setVRCameraOffsetTransform(vrOffset); m_physicsServer.renderScene(); for (int i=0;i<MAX_VR_CONTROLLERS;i++) { if (m_args[0].m_isVrControllerPicking[i] || m_args[0].m_isVrControllerDragging[i]) { btVector3 from = m_args[0].m_vrControllerPos[i]; btMatrix3x3 mat(m_args[0].m_vrControllerOrn[i]); btVector3 toX = from+mat.getColumn(0); btVector3 toY = from+mat.getColumn(1); btVector3 toZ = from+mat.getColumn(2); int width = 2; btVector4 color; color=btVector4(1,0,0,1); m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toX,color,width); color=btVector4(0,1,0,1); m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width); color=btVector4(0,0,1,1); m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width); } } if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera()) { gEnableRealTimeSimVR = true; } if (gDebugRenderToggle) if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera()) { B3_PROFILE("Draw Debug HUD"); //some little experiment to add text/HUD to a VR camera (HTC Vive/Oculus Rift) float pos[4]; m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraTargetPosition(pos); pos[0]+=gVRTeleportPos[0]; pos[1]+=gVRTeleportPos[1]; pos[2]+=gVRTeleportPos[2]; btTransform viewTr; btScalar m[16]; float mf[16]; m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->getCameraViewMatrix(mf); for (int i=0;i<16;i++) { m[i] = mf[i]; } m[12]=+gVRTeleportPos[0]; m[13]=+gVRTeleportPos[1]; m[14]=+gVRTeleportPos[2]; viewTr.setFromOpenGLMatrix(m); btTransform viewTrInv = viewTr.inverse(); btVector3 side = viewTrInv.getBasis().getColumn(0); btVector3 up = viewTrInv.getBasis().getColumn(1); btVector3 fwd = viewTrInv.getBasis().getColumn(2); float upMag = 0; float sideMag = 2.2; float fwdMag = -4; m_guiHelper->getAppInterface()->drawText3D(line0,pos[0]+upMag*up[0]-sideMag*side[0]+fwdMag*fwd[0],pos[1]+upMag*up[1]-sideMag*side[1]+fwdMag*fwd[1],pos[2]+upMag*up[2]-sideMag*side[2]+fwdMag*fwd[2],1); //btVector3 fwd = viewTrInv.getBasis().getColumn(2); up = viewTrInv.getBasis().getColumn(1); upMag = -0.3; m_guiHelper->getAppInterface()->drawText3D(line1,pos[0]+upMag*up[0]-sideMag*side[0]+fwdMag*fwd[0],pos[1]+upMag*up[1]-sideMag*side[1]+fwdMag*fwd[1],pos[2]+upMag*up[2]-sideMag*side[2]+fwdMag*fwd[2],1); } //m_args[0].m_cs->unlock(); }
void PhysicsServerExample::renderScene() { #if 0 ///little VR test to follow/drive Husky vehicle if (gHuskyId >= 0) { gVRTeleportPos1 = huskyTr.getOrigin(); gVRTeleportOrn = huskyTr.getRotation(); } #endif B3_PROFILE("PhysicsServerExample::RenderScene"); drawUserDebugLines(); if (gEnableRealTimeSimVR) { static int frameCount=0; static btScalar prevTime = m_clock.getTimeSeconds(); frameCount++; static btScalar worseFps = 1000000; int numFrames = 200; static int count = 0; count++; #if 0 if (0 == (count & 1)) { btScalar curTime = m_clock.getTimeSeconds(); btScalar fps = 1. / (curTime - prevTime); prevTime = curTime; if (fps < worseFps) { worseFps = fps; } if (count > numFrames) { count = 0; sprintf(line0, "fps:%f frame:%d", worseFps, frameCount / 2); sprintf(line1, "drop:%d tscale:%f dt:%f, substep %f)", gDroppedSimulationSteps, simTimeScalingFactor,gDtInSec, gSubStep); gDroppedSimulationSteps = 0; worseFps = 1000000; } } #endif #ifdef BT_ENABLE_VR if ((gInternalSimFlags&2 ) && m_tinyVrGui==0) { ComboBoxParams comboParams; comboParams.m_comboboxId = 0; comboParams.m_numItems = 0; comboParams.m_startItem = 0; comboParams.m_callback = 0;//MyComboBoxCallback; comboParams.m_userPointer = 0;//this; m_tinyVrGui = new TinyVRGui(comboParams,this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()); m_tinyVrGui->init(); } if (m_tinyVrGui) { b3Transform tr;tr.setIdentity(); tr.setOrigin(b3MakeVector3(gVRController2Pos[0],gVRController2Pos[1],gVRController2Pos[2])); tr.setRotation(b3Quaternion(gVRController2Orn[0],gVRController2Orn[1],gVRController2Orn[2],gVRController2Orn[3])); tr = tr*b3Transform(b3Quaternion(0,0,-SIMD_HALF_PI),b3MakeVector3(0,0,0)); b3Scalar dt = 0.01; m_tinyVrGui->clearTextArea(); static char line0[1024]; static char line1[1024]; m_tinyVrGui->grapicalPrintf(line0,0,0,0,0,0,255); m_tinyVrGui->grapicalPrintf(line1,0,16,255,255,255,255); m_tinyVrGui->tick(dt,tr); } #endif//BT_ENABLE_VR } ///debug rendering //m_args[0].m_cs->lock(); //gVRTeleportPos[0] += 0.01; btTransform tr2a, tr2; tr2a.setIdentity(); tr2.setIdentity(); tr2.setOrigin(gVRTeleportPos1); tr2a.setRotation(gVRTeleportOrn); btTransform trTotal = tr2*tr2a; btTransform trInv = trTotal.inverse(); btMatrix3x3 vrOffsetRot; vrOffsetRot.setRotation(trInv.getRotation()); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { vrOffset[i + 4 * j] = vrOffsetRot[i][j]; } } vrOffset[12]= trInv.getOrigin()[0]; vrOffset[13]= trInv.getOrigin()[1]; vrOffset[14]= trInv.getOrigin()[2]; this->m_multiThreadedHelper->m_childGuiHelper->getRenderInterface()-> getActiveCamera()->setVRCameraOffsetTransform(vrOffset); m_physicsServer.renderScene(); for (int i=0;i<MAX_VR_CONTROLLERS;i++) { if (m_args[0].m_isVrControllerPicking[i] || m_args[0].m_isVrControllerDragging[i]) { btVector3 from = m_args[0].m_vrControllerPos[i]; btMatrix3x3 mat(m_args[0].m_vrControllerOrn[i]); btVector3 toX = from+mat.getColumn(0); btVector3 toY = from+mat.getColumn(1); btVector3 toZ = from+mat.getColumn(2); int width = 2; btVector4 color; color=btVector4(1,0,0,1); m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toX,color,width); color=btVector4(0,1,0,1); m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toY,color,width); color=btVector4(0,0,1,1); m_guiHelper->getAppInterface()->m_renderer->drawLine(from,toZ,color,width); } } if (m_guiHelper->getAppInterface()->m_renderer->getActiveCamera()->isVRCamera()) { if (!gEnableRealTimeSimVR) { gEnableRealTimeSimVR = true; m_physicsServer.enableRealTimeSimulation(1); } } //m_args[0].m_cs->unlock(); }
int b3GpuNarrowPhase::registerCompoundShape(b3AlignedObjectArray<b3GpuChildShape>* childShapes) { int collidableIndex = allocateCollidable(); if (collidableIndex<0) return collidableIndex; b3Collidable& col = getCollidableCpu(collidableIndex); col.m_shapeType = SHAPE_COMPOUND_OF_CONVEX_HULLS; col.m_shapeIndex = m_data->m_cpuChildShapes.size(); { b3Assert(col.m_shapeIndex+childShapes->size()<m_data->m_config.m_maxCompoundChildShapes); for (int i=0;i<childShapes->size();i++) { m_data->m_cpuChildShapes.push_back(childShapes->at(i)); } } col.m_numChildShapes = childShapes->size(); b3SapAabb aabbWS; b3Vector3 myAabbMin(1e30f,1e30f,1e30f); b3Vector3 myAabbMax(-1e30f,-1e30f,-1e30f); //compute local AABB of the compound of all children for (int i=0;i<childShapes->size();i++) { int childColIndex = childShapes->at(i).m_shapeIndex; b3Collidable& childCol = getCollidableCpu(childColIndex); b3SapAabb aabbLoc =m_data->m_localShapeAABBCPU->at(childColIndex); b3Vector3 childLocalAabbMin(aabbLoc.m_min[0],aabbLoc.m_min[1],aabbLoc.m_min[2]); b3Vector3 childLocalAabbMax(aabbLoc.m_max[0],aabbLoc.m_max[1],aabbLoc.m_max[2]); b3Vector3 aMin,aMax; b3Scalar margin(0.f); b3Transform childTr; childTr.setIdentity(); childTr.setOrigin(b3Vector3(childShapes->at(i).m_childPosition[0], childShapes->at(i).m_childPosition[1], childShapes->at(i).m_childPosition[2])); childTr.setRotation(b3Quaternion(childShapes->at(i).m_childOrientation[0], childShapes->at(i).m_childOrientation[1], childShapes->at(i).m_childOrientation[2], childShapes->at(i).m_childOrientation[3])); b3TransformAabb(childLocalAabbMin,childLocalAabbMax,margin,childTr,aMin,aMax); myAabbMin.setMin(aMin); myAabbMax.setMax(aMax); } aabbWS.m_min[0] = myAabbMin[0];//s_convexHeightField->m_aabb.m_min.x; aabbWS.m_min[1]= myAabbMin[1];//s_convexHeightField->m_aabb.m_min.y; aabbWS.m_min[2]= myAabbMin[2];//s_convexHeightField->m_aabb.m_min.z; aabbWS.m_minIndices[3] = 0; aabbWS.m_max[0] = myAabbMax[0];//s_convexHeightField->m_aabb.m_max.x; aabbWS.m_max[1]= myAabbMax[1];//s_convexHeightField->m_aabb.m_max.y; aabbWS.m_max[2]= myAabbMax[2];//s_convexHeightField->m_aabb.m_max.z; aabbWS.m_signedMaxIndices[3] = 0; m_data->m_localShapeAABBCPU->push_back(aabbWS); // m_data->m_localShapeAABBGPU->push_back(aabbWS); clFinish(m_queue); return collidableIndex; }