void PhysicsVisualization::setEnvironmentToDraw(PhysicsEnvironment *env) {
// dummy implementation
std::unique_lock<std::mutex> lock(m_mutex);
if (nullptr != g_envToDraw && nullptr != g_drawingThread)
{
// quit current simulation
dsStop();
g_drawingThread->join();
delete g_drawingThread;
g_drawingThread = nullptr;
}
g_envToDraw = env;
if (nullptr != g_envToDraw)
{
// setup drawing stuff
g_fn.version = DS_VERSION;
g_fn.start = &start;
g_fn.step = &drawFunctionWrapper;
g_fn.stop = &stop;
g_fn.command = 0;
g_fn.path_to_textures = "textures/";
g_drawingThread = new std::thread(dsSimulationLoop, 0, (char**)NULL, 640, 480, &g_fn);
}
}
static void simLoop (int pause)
{
// stop after a given number of iterations, as long as we are not in
// interactive mode
if (cmd_graphics && !cmd_interactive &&
(iteration >= max_iterations)) {
dsStop();
return;
}
iteration++;
if (!pause) {
// do stuff for this test and check to see if the joint is behaving well
dReal error = doStuffAndGetError (test_num);
if (error > max_error) max_error = error;
if (cmd_interactive && error < dInfinity) {
printf ("scaled error = %.4e\n",error);
}
// take a step
dWorldStep (world,STEPSIZE);
// occasionally re-orient the first body to create a deliberate error.
if (cmd_occasional_error) {
static int count = 0;
if ((count % 20)==0) {
// randomly adjust orientation of body[0]
const dReal *R1;
dMatrix3 R2,R3;
R1 = dBodyGetRotation (body[0]);
dRFromAxisAndAngle (R2,dRandReal()-0.5,dRandReal()-0.5,
dRandReal()-0.5,dRandReal()-0.5);
dMultiply0 (R3,R1,R2,3,3,3);
dBodySetRotation (body[0],R3);
// randomly adjust position of body[0]
const dReal *pos = dBodyGetPosition (body[0]);
dBodySetPosition (body[0],
pos[0]+0.2*(dRandReal()-0.5),
pos[1]+0.2*(dRandReal()-0.5),
pos[2]+0.2*(dRandReal()-0.5));
}
count++;
}
}
if (cmd_graphics) {
dReal sides1[3] = {SIDE,SIDE,SIDE};
dReal sides2[3] = {SIDE*0.99f,SIDE*0.99f,SIDE*0.99f};
dsSetTexture (DS_WOOD);
dsSetColor (1,1,0);
dsDrawBox (dBodyGetPosition(body[0]),dBodyGetRotation(body[0]),sides1);
if (body[1]) {
dsSetColor (0,1,1);
dsDrawBox (dBodyGetPosition(body[1]),dBodyGetRotation(body[1]),sides2);
}
}
}
//--------------------------------------------------------------------------------------------
// 関数名 ： dsStreamStop
// 作成者 ： 植山沙欧
// 作成日 ： 2011.03.07
// 機能 ： ストリーミング再生の停止制御を行います
// 引数 ： ＜入力＞numBuffer 書き込むバッファ番号
// ＜入力＞data_ptr データ領域
// ＜入力＞dwSize 読み込むサイズ
// ＜入力＞iWriteOffset 書き込みバッファのオフセット
// 更新履歴 ： 2011.03.07 Ver1.00 植山沙欧 機能の実現
//--------------------------------------------------------------------------------------------
static bool dsStreamStop(int numBuffer, BYTE* data_ptr, DWORD dwSize, int iWriteOffset)
{
bool bRet = false;
if(g_sinfo[numBuffer].effectflag == eEFFECTDS_STOPREADY)
{
ZeroMemory(data_ptr, dwSize);
dsWriteSoundData(numBuffer, data_ptr, iWriteOffset, dwSize);
g_sinfo[numBuffer].effectflag = eEFFECTDS_STOP;
bRet = true;
}
else if(g_sinfo[numBuffer].effectflag == eEFFECTDS_STOP)
{
dsStop(numBuffer);
g_sinfo[numBuffer].effectflag = eEFFECTDS_NOTHING;
bRet = true;
}
return bRet;
}
/*** Simulation loop ***/
void simLoop(int pause)
{
if (!pause and !pauseGlobal) {
if(shouldIKeepRunningTheSimulation()){
walk(); // control of walking
dSpaceCollide(space,0,&nearCallback); // collision detection
#ifndef USE_NLEG_ROBOT
dWorldStep(world, 0.001); // jmc: original was 0.01 but it was crashing
#else
dWorldStep(world, 0.001);
// dWorldQuickStep(world, 0.001); // jmc: original was 0.01 but it was crashing
#endif
dJointGroupEmpty(contactgroup); // Point of contact group sky
}
#ifndef NOVIZ
else dsStop(); //end the simulation
#endif
}
#ifdef USE_NLEG_ROBOT
drawRobot_Nleg();
#else
drawRobot();
#endif
}
// simulation loop
static void simLoop (int pause)
{
static bool todo = false;
if ( todo ) { // DEBUG
static int cnt = 0;
++cnt;
if (cnt == 5)
command ( 'q' );
if (cnt == 10)
dsStop();
}
if (!pause) {
double simstep = 0.01; // 10ms simulation steps
double dt = dsElapsedTime();
int nrofsteps = (int) ceilf (dt/simstep);
if (!nrofsteps)
nrofsteps = 1;
for (int i=0; i<nrofsteps && !pause; i++) {
dSpaceCollide (space,0,&nearCallback);
dWorldStep (world, simstep);
dJointGroupEmpty (contactgroup);
}
update();
dReal radius, length;
dsSetTexture (DS_WOOD);
drawBox (geom[W], 1,1,0);
drawBox (geom[EXT], 0,1,0);
dVector3 anchorPos;
dReal ang1 = 0;
dReal ang2 = 0;
dVector3 axisP, axisR1, axisR2;
if ( dJointTypePU == type ) {
dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
ang1 = pu->getAngle1();
ang2 = pu->getAngle2();
pu->getAxis1 (axisR1);
pu->getAxis2 (axisR2);
pu->getAxisP (axisP);
dJointGetPUAnchor (pu->id(), anchorPos);
}
else if ( dJointTypePR == type ) {
dPRJoint *pr = dynamic_cast<dPRJoint *> (joint);
pr->getAxis1 (axisP);
pr->getAxis2 (axisR1);
dJointGetPRAnchor (pr->id(), anchorPos);
}
// Draw the axisR
if ( geom[INT] ) {
dsSetColor (1,0,1);
dVector3 l;
dGeomBoxGetLengths (geom[INT], l);
const dReal *rotBox = dGeomGetRotation (geom[W]);
dVector3 pos;
for (int i=0; i<3; ++i)
pos[i] = anchorPos[i] - 0.5*extDim[Z]*axisP[i];
dsDrawBox (pos, rotBox, l);
}
dsSetTexture (DS_CHECKERED);
if ( geom[AXIS1] ) {
dQuaternion q, qAng;
dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
dGeomGetQuaternion (geom[AXIS1], q);
dQuaternion qq;
dQMultiply1 (qq, qAng, q);
dMatrix3 R;
dQtoR (qq,R);
dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS1]), &radius, &length);
dsSetColor (1,0,0);
dsDrawCylinder (anchorPos, R, length, radius);
}
if ( dJointTypePU == type && geom[AXIS2] ) {
//dPUJoint *pu = dynamic_cast<dPUJoint *> (joint);
dQuaternion q, qAng, qq, qq1;
dGeomGetQuaternion (geom[AXIS2], q);
dQFromAxisAndAngle (qAng, 0, 1, 0, ang2);
dQMultiply1 (qq, qAng, q);
dQFromAxisAndAngle (qAng,axisR1[X], axisR1[Y], axisR1[Z], ang1);
dQMultiply1 (qq1, qAng, qq);
dMatrix3 R;
dQtoR (qq1,R);
dGeomCylinderGetParams (dGeomTransformGetGeom (geom[AXIS2]), &radius, &length);
dsSetColor (0,0,1);
dsDrawCylinder (anchorPos, R, length, radius);
}
dsSetTexture (DS_WOOD);
// Draw the anchor
if ( geom[ANCHOR] ) {
dsSetColor (1,1,1);
dVector3 l;
dGeomBoxGetLengths (geom[ANCHOR], l);
const dReal *rotBox = dGeomGetRotation (geom[D]);
const dReal *posBox = dGeomGetPosition (geom[D]);
dVector3 e;
for (int i=0; i<3; ++i)
e[i] = posBox[i] - anchorPos[i];
dNormalize3 (e);
dVector3 pos;
for (int i=0; i<3; ++i)
pos[i] = anchorPos[i] + 0.5 * l[Z]*e[i];
dsDrawBox (pos, rotBox, l);
}
drawBox (geom[D], 1,1,0);
}
}
IoObject *IoDrawStuff_dsStop(IoDrawStuff *self, IoObject *locals, IoMessage *m)
{
dsStop();
return self;
}
//--------------------------------------------------------------------------------------------
// 関数名 ： dsCheck_Stream
// 作成者 ： 植山沙欧
// 作成日 ： 2002.06.24
// 機能 ： ストリーミングをチェックします
// 必ず毎フレーム呼び出してください
// 引数 ： ありません
// 使用変数 ： int iCnt ループカウンタ
// DWORD dwStatus 再生フラグを格納します
// DWORD dwPos 再生ポジションを格納します
// 使用関数 ： IDirectSoundBuffer8::GetStatus バッファの状態を取得します
// IDirectSoundBuffer8::GetCurrentPosition 現在のバッファの再生ポジションを取得します
// dsLoadStream ストリームングを行ないます
// dsStop 再生中のバッファを停止させます
// dsSetVolume ボリュームを調整します（フェードイン、アウト用）
// 戻り値 ： ありません
// 更新履歴 ： 2002.06.24 Ver1.00 植山沙欧 機能の実現
// 2002.10.27 Ver1.01 植山沙欧 ストリーミングしないサウンドをサポート
//--------------------------------------------------------------------------------------------
void dsCheck_Stream(void)
{
int iCnt; // ループカウンタ
DWORD dwStatus; // 再生フラグを格納
DWORD dwPos; // 再生ポジションを格納
// 読み込みタイミングを調べる
for(iCnt=0;iCnt<DS_MAX_BUFFER;iCnt++)
{
// 初期化が終わっていない場合
if(!g_ds.sec_ptr[iCnt])
{
continue;
}
// エフェクトを管理
if(g_sinfo[iCnt].effectflag == eEFFECTDS_FADEOUT)
{
g_sinfo[iCnt].iVolume -= g_sinfo[iCnt].iFade;//DS_VALUE_FADE;
if(g_sinfo[iCnt].iVolume < DS_VOLUME_MIN)
{
if (g_sinfo[iCnt].fadeout_stop)
dsStop(iCnt);
g_sinfo[iCnt].effectflag = eEFFECTDS_NOTHING;
}
dsSetVolume(iCnt, g_sinfo[iCnt].iVolume);
}
if(g_sinfo[iCnt].effectflag == eEFFECTDS_FADEIN)
{
g_sinfo[iCnt].iVolume += g_sinfo[iCnt].iFade;//DS_VALUE_FADE;
if(g_sinfo[iCnt].iVolume > DSBVOLUME_MAX)
{
g_sinfo[iCnt].iVolume = DSBVOLUME_MAX;
g_sinfo[iCnt].effectflag = eEFFECTDS_NOTHING;
}
dsSetVolume(iCnt, g_sinfo[iCnt].iVolume);
}
// ストリーミング再生が許可されていない
if(!g_sinfo[iCnt].bStream)
{
continue;
}
// バッファは再生中かどうかを調べる
g_ds.sec_ptr[iCnt]->GetStatus(&dwStatus);
if(!(dwStatus & DSBSTATUS_PLAYING))
{
continue;
}
// 読み込みタイミングかどうかをチェック
g_ds.sec_ptr[iCnt]->GetCurrentPosition(&dwPos, NULL);
// 再生ポジションフラグが0で、かつ0-11025なら22050-44100のデータを更新する。
if((g_sinfo[iCnt].iStreamPos == 0) && (dwPos > 0) && (dwPos < (g_ds.dwBufferSize / 2)))
{
dsLoadStream(iCnt, g_ds.dwBufferSize, 1);
}
// 再生ポジションフラグが1で、かつ22050-33075なら0-22050のデータを更新。
if((g_sinfo[iCnt].iStreamPos == 1) && (dwPos > g_ds.dwBufferSize))
{
dsLoadStream(iCnt, 0, 0);
}
}
}
