void doFlags()
{
//float ms = getDeltaTimeMicroseconds();
btScalar dt = (btScalar)m_clock.getTimeMicroseconds();
m_clock.reset();
///step the simulation
if( m_dynamicsWorld )
{
m_dynamicsWorld->stepSimulation(dt/1000000.);
static int frameCount = 0;
frameCount++;
if (frameCount==100)
{
m_dynamicsWorld->stepSimulation(1./60.,0);
CProfileManager::dumpAll();
}
updatePhysicsWorld();
//m_dynamicsWorld->setDebugDrawer(&debugDraw);
//debugDraw.setDebugMode(btIDebugDraw::DBG_DrawWireframe);
//g_solver->copyBackToSoftBodies();
//m_dynamicsWorld->debugDrawWorld();
}
for( int flagIndex = 0; flagIndex < m_flags.size(); ++flagIndex )
{
g_softBodyOutput->copySoftBodyToVertexBuffer( m_flags[flagIndex], cloths[flagIndex].m_vertexBufferDescriptor );
cloths[flagIndex].draw();
}
}
void cast (btCollisionWorld* cw)
{
#ifdef USE_BT_CLOCK
frame_timer.reset ();
#endif //USE_BT_CLOCK
#ifdef BATCH_RAYCASTER
if (!gBatchRaycaster)
return;
gBatchRaycaster->clearRays ();
for (int i = 0; i < NUMRAYS_IN_BAR; i++)
{
gBatchRaycaster->addRay (source[i], dest[i]);
}
gBatchRaycaster->performBatchRaycast ();
for (int i = 0; i < gBatchRaycaster->getNumRays (); i++)
{
const SpuRaycastTaskWorkUnitOut& out = (*gBatchRaycaster)[i];
hit[i].setInterpolate3(source[i],dest[i],out.hitFraction);
normal[i] = out.hitNormal;
normal[i].normalize ();
}
#else
for (int i = 0; i < NUMRAYS_IN_BAR; i++)
{
btCollisionWorld::ClosestRayResultCallback cb(source[i], dest[i]);
cw->rayTest (source[i], dest[i], cb);
if (cb.hasHit ())
{
hit[i] = cb.m_hitPointWorld;
normal[i] = cb.m_hitNormalWorld;
normal[i].normalize ();
} else {
hit[i] = dest[i];
normal[i] = btVector3(1.0, 0.0, 0.0);
}
}
#ifdef USE_BT_CLOCK
ms += frame_timer.getTimeMilliseconds ();
#endif //USE_BT_CLOCK
frame_counter++;
if (frame_counter > 50)
{
min_ms = ms < min_ms ? ms : min_ms;
max_ms = ms > max_ms ? ms : max_ms;
sum_ms += ms;
sum_ms_samples++;
btScalar mean_ms = (btScalar)sum_ms/(btScalar)sum_ms_samples;
printf("%d rays in %d ms %d %d %f\n", NUMRAYS_IN_BAR * frame_counter, ms, min_ms, max_ms, mean_ms);
ms = 0;
frame_counter = 0;
}
#endif
}
void cast (btCollisionWorld* cw)
{
#ifdef USE_BT_CLOCK
frame_timer.reset ();
#endif //USE_BT_CLOCK
for (int i = 0; i < NUMRAYS_IN_BAR; i++)
{
btCollisionWorld::ClosestConvexResultCallback cb(source[i], dest[i]);
btQuaternion qFrom;
btQuaternion qTo;
qFrom.setRotation (btVector3(1.0, 0.0, 0.0), 0.0);
qTo.setRotation (btVector3(1.0, 0.0, 0.0), 0.7);
btTransform from(qFrom, source[i]);
btTransform to(qTo, dest[i]);
cw->convexSweepTest (&boxShape, from, to, cb);
if (cb.hasHit ())
{
hit_surface[i] = cb.m_hitPointWorld;
hit_com[i].setInterpolate3(source[i], dest[i], cb.m_closestHitFraction);
hit_fraction[i] = cb.m_closestHitFraction;
normal[i] = cb.m_hitNormalWorld;
normal[i].normalize ();
} else {
hit_com[i] = dest[i];
hit_surface[i] = dest[i];
hit_fraction[i] = 1.0f;
normal[i] = btVector3(1.0, 0.0, 0.0);
}
}
#ifdef USE_BT_CLOCK
ms += frame_timer.getTimeMilliseconds ();
#endif //USE_BT_CLOCK
frame_counter++;
if (frame_counter > 50)
{
min_ms = ms < min_ms ? ms : min_ms;
max_ms = ms > max_ms ? ms : max_ms;
sum_ms += ms;
sum_ms_samples++;
btScalar mean_ms = (btScalar)sum_ms/(btScalar)sum_ms_samples;
printf("%d rays in %d ms %d %d %f\n", NUMRAYS_IN_BAR * frame_counter, ms, min_ms, max_ms, mean_ms);
ms = 0;
frame_counter = 0;
}
}
/***********************************************************************************************
* CProfileManager::Reset -- Reset the contents of the profiling system *
* *
* This resets everything except for the tree structure. All of the timing data is reset. *
*=============================================================================================*/
void CProfileManager::Reset( void )
{
gProfileClock.reset();
Root.Reset();
Root.Call();
FrameCounter = 0;
Profile_Get_Ticks(&ResetTime);
}
/***********************************************************************************************
* CProfileManager::Reset -- Reset the contents of the profiling system *
* *
* This resets everything except for the tree structure. All of the timing data is reset. *
*=============================================================================================*/
void CProfileManager::Reset( void )
{
if(!m_profilerEnabled) return;
gProfileClock.reset();
Root.Reset();
Root.Call();
FrameCounter = 0;
Profile_Get_Ticks(&ResetTime);
}
void doFlags()
{
//float ms = getDeltaTimeMicroseconds();
btScalar dt = (btScalar)m_clock.getTimeMicroseconds();
m_clock.reset();
///step the simulation
if( m_dynamicsWorld )
{
m_dynamicsWorld->stepSimulation(dt/1000000.);
static int frameCount = 0;
frameCount++;
if (frameCount==100)
{
m_dynamicsWorld->stepSimulation(1./60.,0);
// Option to save a .bullet file
// btDefaultSerializer* serializer = new btDefaultSerializer();
// m_dynamicsWorld->serialize(serializer);
// FILE* file = fopen("testFile.bullet","wb");
// fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1, file);
// fclose(file);
CProfileManager::dumpAll();
}
updatePhysicsWorld();
//m_dynamicsWorld->setDebugDrawer(&debugDraw);
//debugDraw.setDebugMode(btIDebugDraw::DBG_DrawWireframe);
//g_solver->copyBackToSoftBodies();
m_dynamicsWorld->debugDrawWorld();
}
for( int flagIndex = 0; flagIndex < m_flags.size(); ++flagIndex )
{
if (g_softBodyOutput)
g_softBodyOutput->copySoftBodyToVertexBuffer( m_flags[flagIndex], cloths[flagIndex].m_vertexBufferDescriptor );
cloths[flagIndex].draw();
}
}
static void OutputTime(const char* name, btClock& c, unsigned count = 0)
{
const unsigned long us = c.getTimeMicroseconds();
const unsigned long ms = (us + 500) / 1000;
const btScalar sec = us / (btScalar)(1000 * 1000);
if (count > 0)
printf("%s : %u us (%u ms), %.2f/s\r\n", name, us, ms, count / sec);
else
printf("%s : %u us (%u ms)\r\n", name, us, ms);
}
/***********************************************************************************************
* CProfileManager::Reset -- Reset the contents of the profiling system *
* *
* This resets everything except for the tree structure. All of the timing data is reset. *
*=============================================================================================*/
void CProfileManager::Reset( void )
{
gProfileClock.reset();
int threadIndex = btQuickprofGetCurrentThreadIndex2();
if (threadIndex<0)
return;
gRoots[threadIndex].Reset();
gRoots[threadIndex].Call();
FrameCounter = 0;
Profile_Get_Ticks(&ResetTime);
}
/***********************************************************************************************
* CProfileManager::Reset -- Reset the contents of the profiling system *
* *
* This resets everything except for the tree structure. All of the timing data is reset. *
*=============================================================================================*/
void CProfileManager::Reset( void )
{
gProfileClock.reset();
int threadIndex = btQuickprofGetCurrentThreadIndex2();
if ((threadIndex<0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT)
return;
gRoots[threadIndex].Reset();
gRoots[threadIndex].Call();
FrameCounter = 0;
Profile_Get_Ticks(&ResetTime);
}
void physics_simulate()
{
//run the simulation
static btClock clock;
static bool first = true;
if (first)
{
first=false;
clock.reset();
}
btScalar dt = (btScalar)clock.getTimeMicroseconds();
clock.reset();
m_dynamicsWorld->stepSimulation(dt/1000000.f);
int i;
for (i=0;i<m_dynamicsWorld->getNumCollisionObjects();i++)
{
btRigidBody* body = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[i]);
if (body)
{
PfxRigidState* state = (PfxRigidState*) body->getUserPointer();
PfxVector3 pe_pos = getVmVector3(body->getWorldTransform().getOrigin());
PfxQuat pe_orn = getVmQuat(body->getWorldTransform().getRotation());
PfxVector3 pe_lvel = getVmVector3(body->getLinearVelocity());
PfxVector3 pe_avel = getVmVector3(body->getAngularVelocity());
state->setPosition(pe_pos);
state->setOrientation(pe_orn);
state->setLinearVelocity(pe_lvel);
state->setAngularVelocity(pe_avel);
}
}
}
void CProfileNode::Reset( void )
{
TotalCalls = 0;
TotalTime = 0.0f;
gProfileClock.reset();
if ( Child ) {
Child->Reset();
}
if ( Sibling ) {
Sibling->Reset();
}
}
void MyLeaveProfileZoneFunc()
{
if (gProfileDisabled)
return;
#ifndef BT_NO_PROFILE
int threadId = btQuickprofGetCurrentThreadIndex2();
if (threadId < 0 || threadId >= BT_QUICKPROF_MAX_THREAD_COUNT)
return;
if (gStackDepths[threadId] <= 0)
{
return;
}
gStackDepths[threadId]--;
const char* name = gFuncNames[threadId][gStackDepths[threadId]];
unsigned long long int startTime = gStartTimes[threadId][gStackDepths[threadId]];
unsigned long long int endTime = clk.getTimeNanoseconds();
gTimings[threadId].addTiming(name, threadId, startTime, endTime);
#endif //BT_NO_PROFILE
}
void MyEnterProfileZoneFunc(const char* msg)
{
if (gProfileDisabled)
return;
#ifndef BT_NO_PROFILE
int threadId = btQuickprofGetCurrentThreadIndex2();
if (threadId < 0 || threadId >= BT_QUICKPROF_MAX_THREAD_COUNT)
return;
if (gStackDepths[threadId] >= MAX_NESTING)
{
btAssert(0);
return;
}
gFuncNames[threadId][gStackDepths[threadId]] = msg;
gStartTimes[threadId][gStackDepths[threadId]] = clk.getTimeNanoseconds();
if (gStartTimes[threadId][gStackDepths[threadId]] <= gStartTimes[threadId][gStackDepths[threadId] - 1])
{
gStartTimes[threadId][gStackDepths[threadId]] = 1 + gStartTimes[threadId][gStackDepths[threadId] - 1];
}
gStackDepths[threadId]++;
#endif
}
void bt_end_gim02_tri_time()
{
g_accum_triangle_collision_time += g_triangle_clock.getTimeMicroseconds();
g_count_triangle_collision++;
}
void bt_begin_gim02_tri_time()
{
g_triangle_clock.reset();
}
//--------------------------------------------------------------------------------------
// Render the scene using the D3D11 device
//--------------------------------------------------------------------------------------
void CALLBACK OnD3D11FrameRender( ID3D11Device* pd3dDevice, ID3D11DeviceContext* pd3dImmediateContext, double fTime,
float fElapsedTime, void* pUserContext )
{
//float ms = getDeltaTimeMicroseconds();
btScalar dt = (btScalar)m_clock.getTimeMicroseconds();
m_clock.reset();
///step the simulation
if (m_dynamicsWorld && !paused)
{
m_dynamicsWorld->stepSimulation(dt / 1000000.f);
updatePhysicsWorld();
}
//paused = 1;
///////////////////////////////////////////////////////
HRESULT hr;
// If the settings dialog is being shown, then render it instead of rendering the app's scene
if( g_D3DSettingsDlg.IsActive() )
{
g_D3DSettingsDlg.OnRender( fElapsedTime );
return;
}
// Clear the render target and depth stencil
float ClearColor[4] = { 0.0f, 0.25f, 0.25f, 0.55f };
ID3D11RenderTargetView* pRTV = DXUTGetD3D11RenderTargetView();
pd3dImmediateContext->ClearRenderTargetView( pRTV, ClearColor );
ID3D11DepthStencilView* pDSV = DXUTGetD3D11DepthStencilView();
pd3dImmediateContext->ClearDepthStencilView( pDSV, D3D11_CLEAR_DEPTH, 1.0, 0 );
for( int flagIndex = 0; flagIndex < m_flags.size(); ++flagIndex )
{
g_softBodyOutput->copySoftBodyToVertexBuffer( m_flags[flagIndex], cloths[flagIndex].m_vertexBufferDescriptor );
cloths[flagIndex].draw();
}
my_capsule.draw();
DXUT_BeginPerfEvent( DXUT_PERFEVENTCOLOR, L"HUD / Stats" );
g_HUD.OnRender( fElapsedTime );
g_SampleUI.OnRender( fElapsedTime );
RenderText();
DXUT_EndPerfEvent();
/*
SAFE_RELEASE(pRTV);
SAFE_RELEASE(pDSV);
*/
}
__forceinline ProfileScope(btClock& clock, unsigned long& value) : m_clock(&clock), m_value(&value), m_base(clock.getTimeMicroseconds())
{
}
void kSetupContact(btParallelConstraintSolver* pSolver,
btParallelConstraintSolverSetupTaskParams* pParams,
btContactSolverInfo* pInfoGlobal, int threadId)
{
int numConstraints = pParams[threadId].m_numContactConstraints;
unsigned long int timeStamp;
int startIndex = pParams[threadId].m_startIndex;
btContactSolverInfo& infoGlobal = *pInfoGlobal;
for(int i = 0; i < numConstraints; i++)
{
timeStamp = sClock.getTimeMicroseconds();
btSolverConstraint& solverConstraint = pSolver->m_tmpSolverContactConstraintPool[startIndex + i];
solverConstraint.m_numConsecutiveRowsPerKernel = timeStamp;
btCollisionObject* colObj0 = (btCollisionObject*)solverConstraint.m_solverBodyA;
btCollisionObject* colObj1 = (btCollisionObject*)solverConstraint.m_solverBodyB;
btRigidBody* solverBodyA = btRigidBody::upcast(colObj0);
btRigidBody* solverBodyB = btRigidBody::upcast(colObj1);
btManifoldPoint& cp = *((btManifoldPoint*)(solverConstraint.m_originalContactPoint));
btVector3 rel_pos1;
btVector3 rel_pos2;
btScalar relaxation;
btScalar rel_vel;
btVector3 vel;
pSolver->setupContactConstraint(solverConstraint, colObj0, colObj1, cp, infoGlobal, vel, rel_vel, relaxation, rel_pos1, rel_pos2);
int currFrictIndex = solverConstraint.m_frictionIndex;
if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !cp.m_lateralFrictionInitialized)
{
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
btScalar lat_rel_vel = cp.m_lateralFrictionDir1.length2();
if(!(infoGlobal.m_solverMode & SOLVER_DISABLE_VELOCITY_DEPENDENT_FRICTION_DIRECTION) && lat_rel_vel > SIMD_EPSILON)
{
cp.m_lateralFrictionDir1 /= btSqrt(lat_rel_vel);
if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
cp.m_lateralFrictionDir2 = cp.m_lateralFrictionDir1.cross(cp.m_normalWorldOnB);
cp.m_lateralFrictionDir2.normalize();//??
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex];
currFrictIndex++;
pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex];
currFrictIndex++;
pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
cp.m_lateralFrictionInitialized = true;
}
else
{
//re-calculate friction direction every frame, todo: check if this is really needed
btPlaneSpace1(cp.m_normalWorldOnB,cp.m_lateralFrictionDir1,cp.m_lateralFrictionDir2);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2);
btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex];
currFrictIndex++;
pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
}
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1);
btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex];
currFrictIndex++;
pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
cp.m_lateralFrictionInitialized = true;
}
}
else
{
btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex];
currFrictIndex++;
pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir1,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_contactCFM1);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
btSolverConstraint& frictionConstraint = pSolver->m_tmpSolverContactFrictionConstraintPool[currFrictIndex];
currFrictIndex++;
pSolver->setupFrictionConstraint(frictionConstraint, cp.m_lateralFrictionDir2,solverBodyA,solverBodyB,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_contactCFM2);
}
}
pSolver->setFrictionConstraintImpulse( solverConstraint, solverBodyA, solverBodyB, cp, infoGlobal);
}
}
inline void Profile_Get_Ticks(unsigned long int * ticks)
{
*ticks = gProfileClock.getTimeMicroseconds();
}
~ProfileInstance()
{
float time = mClock.getTimeSeconds();
mHistory->addCallTime( time );
}
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();
}
