bool FGGyro::Run(void )
{
// There is no input assumed. This is a dedicated angular acceleration sensor.
//aircraft rates
vAccel = mT * Accelerations->GetPQRdot();
Input = vAccel(axis);
ProcessSensorSignal();
return true;
}
bool FGAccelerometer::Run(void )
{
// There is no input assumed. This is a dedicated acceleration sensor.
vRadius = MassBalance->StructuralToBody(vLocation);
//gravitational forces
vAccel = Propagate->GetTl2b() * FGColumnVector3(0, 0, Inertial->gravity());
//aircraft forces
vAccel += (Accelerations->GetUVWdot()
+ Accelerations->GetPQRdot() * vRadius
+ Propagate->GetPQR() * (Propagate->GetPQR() * vRadius));
// transform to the specified orientation
vAccel = mT * vAccel;
Input = vAccel(axis);
ProcessSensorSignal();
return true;
}
void CGameEngine::HandleInput(float fSeconds)
{
if((GetKeyState(VK_F5) & 0x8000))
{
if((GetKeyState(VK_SHIFT) & 0x8000))
m_Kr = Max(0.0f, m_Kr - 0.0001f);
else
m_Kr += 0.0001f;
m_Kr4PI = m_Kr*4.0f*PI;
}
else if((GetKeyState(VK_F6) & 0x8000))
{
if((GetKeyState(VK_SHIFT) & 0x8000))
m_Km = Max(0.0f, m_Km - 0.0001f);
else
m_Km += 0.0001f;
m_Km4PI = m_Km*4.0f*PI;
}
else if((GetKeyState(VK_F7) & 0x8000))
{
if((GetKeyState(VK_SHIFT) & 0x8000))
m_g = Max(-1.0f, m_g-0.01f);
else
m_g = Min(1.0f, m_g+0.01f);
}
else if((GetKeyState(VK_F8) & 0x8000))
{
if((GetKeyState(VK_SHIFT) & 0x8000))
m_ESun = Max(0.0f, m_ESun - 0.1f);
else
m_ESun += 0.1f;
}
else if((GetKeyState(VK_F9) & 0x8000))
{
if((GetKeyState(VK_SHIFT) & 0x8000))
m_fWavelength[0] = Max(0.001f, m_fWavelength[0] -= 0.001f);
else
m_fWavelength[0] += 0.001f;
m_fWavelength4[0] = powf(m_fWavelength[0], 4.0f);
}
else if((GetKeyState(VK_F10) & 0x8000))
{
if((GetKeyState(VK_SHIFT) & 0x8000))
m_fWavelength[1] = Max(0.001f, m_fWavelength[1] -= 0.001f);
else
m_fWavelength[1] += 0.001f;
m_fWavelength4[1] = powf(m_fWavelength[1], 4.0f);
}
else if((GetKeyState(VK_F11) & 0x8000))
{
if((GetKeyState(VK_SHIFT) & 0x8000))
m_fWavelength[2] = Max(0.001f, m_fWavelength[2] -= 0.001f);
else
m_fWavelength[2] += 0.001f;
m_fWavelength4[2] = powf(m_fWavelength[2], 4.0f);
}
const float ROTATE_SPEED = 1.0f;
// Turn left/right means rotate around the up axis
if((GetKeyState(VK_NUMPAD6) & 0x8000) || (GetKeyState(VK_RIGHT) & 0x8000))
m_3DCamera.Rotate(m_3DCamera.GetUpAxis(), fSeconds * -ROTATE_SPEED);
if((GetKeyState(VK_NUMPAD4) & 0x8000) || (GetKeyState(VK_LEFT) & 0x8000))
m_3DCamera.Rotate(m_3DCamera.GetUpAxis(), fSeconds * ROTATE_SPEED);
// Turn up/down means rotate around the right axis
if((GetKeyState(VK_NUMPAD8) & 0x8000) || (GetKeyState(VK_UP) & 0x8000))
m_3DCamera.Rotate(m_3DCamera.GetRightAxis(), fSeconds * -ROTATE_SPEED);
if((GetKeyState(VK_NUMPAD2) & 0x8000) || (GetKeyState(VK_DOWN) & 0x8000))
m_3DCamera.Rotate(m_3DCamera.GetRightAxis(), fSeconds * ROTATE_SPEED);
// Roll means rotate around the view axis
if(GetKeyState(VK_F1) & 0x8000)
m_3DCamera.Rotate(m_3DCamera.GetViewAxis(), fSeconds * -ROTATE_SPEED);
if(GetKeyState(VK_F2) & 0x8000)
m_3DCamera.Rotate(m_3DCamera.GetViewAxis(), fSeconds * ROTATE_SPEED);
#define THRUST 1.0f // Acceleration rate due to thrusters (units/s*s)
#define RESISTANCE 0.1f // Damping effect on velocity
// Handle acceleration keys
CVector vAccel(0.0f);
if(GetKeyState(VK_SPACE) & 0x8000)
m_3DCamera.SetVelocity(CVector(0.0f)); // Full stop
else
{
// Add camera's acceleration due to thrusters
float fThrust = THRUST;
if(GetKeyState(VK_CONTROL) & 0x8000)
fThrust *= 10.0f;
// Thrust forward/reverse affects velocity along the view axis
if(GetKeyState('W') & 0x8000)
vAccel += m_3DCamera.GetViewAxis() * fThrust;
if(GetKeyState('S') & 0x8000)
vAccel += m_3DCamera.GetViewAxis() * -fThrust;
// Thrust left/right affects velocity along the right axis
if(GetKeyState('D') & 0x8000)
vAccel += m_3DCamera.GetRightAxis() * fThrust;
if(GetKeyState('A') & 0x8000)
vAccel += m_3DCamera.GetRightAxis() * -fThrust;
m_3DCamera.Accelerate(vAccel, fSeconds, RESISTANCE);
CVector vPos = m_3DCamera.GetPosition();
float fMagnitude = vPos.Magnitude();
if(fMagnitude < m_fInnerRadius)
{
vPos *= (m_fInnerRadius * (1 + DELTA)) / fMagnitude;
m_3DCamera.SetPosition(CDoubleVector(vPos.x, vPos.y, vPos.z));
m_3DCamera.SetVelocity(-m_3DCamera.GetVelocity());
}
}
}
void CGameEngine::RenderFrame(int nMilliseconds)
{
int i;
// Determine the FPS
static char szFrameCount[20] = {0};
static int nTime = 0;
static int nFrames = 0;
nTime += nMilliseconds;
if(nTime >= 1000)
{
m_fFPS = (float)(nFrames * 1000) / (float)nTime;
sprintf(szFrameCount, "%2.2f FPS", m_fFPS);
nTime = nFrames = 0;
}
nFrames++;
// Move the camera
HandleInput(nMilliseconds * 0.001f);
glClearColor(0, 0, 0, 0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glPushMatrix();
glLoadMatrixf(m_3DCamera.GetViewMatrix());
glLightfv(GL_LIGHT0, GL_POSITION, CVector4(m_vLight.x, m_vLight.y, m_vLight.z, 1));
glDisable(GL_LIGHTING);
C3DObject obj;
glMultMatrixf(obj.GetModelMatrix(&m_3DCamera));
if(m_bShowTexture)
{
CTexture t(&m_pbOpticalDepth);
t.Enable();
glBegin(GL_QUADS);
glTexCoord2f(0, 0);
glVertex3f(-3.0f, 3.0f, 0.0f);
glTexCoord2f(0, 1);
glVertex3f(-3.0f, -3.0f, 0.0f);
glTexCoord2f(1, 1);
glVertex3f(3.0f, -3.0f, 0.0f);
glTexCoord2f(1, 0);
glVertex3f(3.0f, 3.0f, 0.0f);
glEnd();
t.Disable();
}
else
{
// Update the color for the vertices of each sphere
CVector vCamera = m_3DCamera.GetPosition();
SVertex *pBuffer = m_sphereInner.GetVertexBuffer();
for(int i=0; i<m_sphereInner.GetVertexCount(); i++)
{
if((vCamera | pBuffer[i].vPos) > 0) // Cheap optimization: Don't update vertices on the back half of the sphere
SetColor(&pBuffer[i]);
}
pBuffer = m_sphereOuter.GetVertexBuffer();
for(i=0; i<m_sphereOuter.GetVertexCount(); i++)
{
if((vCamera | pBuffer[i].vPos) > 0) // Cheap optimization: Don't update vertices on the back half of the sphere
SetColor(&pBuffer[i]);
}
// Then draw the two spheres
m_sphereInner.Draw();
glFrontFace(GL_CW);
m_sphereOuter.Draw();
glFrontFace(GL_CCW);
}
glPopMatrix();
glEnable(GL_LIGHTING);
// Draw info in the top-left corner
char szBuffer[256];
m_fFont.Begin();
glColor3d(1.0, 1.0, 1.0);
m_fFont.SetPosition(0, 0);
m_fFont.Print(szFrameCount);
m_fFont.SetPosition(0, 15);
/*
sprintf(szBuffer, "Samples (+/-): %d", m_nSamples);
m_fFont.Print(szBuffer);
m_fFont.SetPosition(0, 30);
sprintf(szBuffer, "Kr (F5/Sh+F5): %-4.4f", m_Kr);
m_fFont.Print(szBuffer);
m_fFont.SetPosition(0, 45);
sprintf(szBuffer, "Km (F6/Sh+F6): %-4.4f", m_Km);
m_fFont.Print(szBuffer);
m_fFont.SetPosition(0, 60);
sprintf(szBuffer, "g (F7/Sh+F7): %-2.2f", m_g);
m_fFont.Print(szBuffer);
m_fFont.SetPosition(0, 75);
sprintf(szBuffer, "ESun (F8/Sh+F8): %-1.1f", m_ESun);
m_fFont.Print(szBuffer);
m_fFont.SetPosition(0, 90);
sprintf(szBuffer, "Red (F9/Sh+F9): %-3.3f", m_fWavelength[0]);
m_fFont.Print(szBuffer);
m_fFont.SetPosition(0, 105);
sprintf(szBuffer, "Green (F10/Sh+F10): %-3.3f", m_fWavelength[1]);
m_fFont.Print(szBuffer);
m_fFont.SetPosition(0, 120);
sprintf(szBuffer, "Blue (F11/Sh+F11): %-3.3f", m_fWavelength[2]);
m_fFont.Print(szBuffer);
*/
/*
for (int i = 0; i < users.getSize(); ++i)
{
const nite::UserData& user = users[i];
sampleViewer->updateUserState(user, userTrackerFrame.getTimestamp());
if (user.isNew())
{
sampleViewer->m_pUserTracker->startSkeletonTracking(user.getId());
sampleViewer->m_pUserTracker->startPoseDetection(user.getId(), nite::POSE_CROSSED_HANDS);
}
else if (!user.isLost())
{
sampleViewer->SetRes(depthFrame);
sampleViewer->DrawStatusLabel(sampleViewer->m_pUserTracker, user); // div by 0
sampleViewer->DrawCenterOfMass(sampleViewer->m_pUserTracker, user);
sampleViewer->DrawBoundingBox(user);
sampleViewer->DrawSkeleton(sampleViewer->m_pUserTracker, user);
}
}
*/
//m_fFont.Print(sampleViewer->m_error);
//sampleViewer->Display();
nite::UserTrackerFrameRef userTrackerFrame;
openni::VideoFrameRef depthFrame;
nite::Status rc = sampleViewer->m_pUserTracker->readFrame(&userTrackerFrame);
int skipAmount = int(m_fFPS)*4; // variable skip depending on the frameRate - constant in human time
//int skipAmount = 300;
if (rc != nite::STATUS_OK)
{
printf("GetNextData failed\n");
return;
}
depthFrame = userTrackerFrame.getDepthFrame();
const nite::UserMap& userLabels = userTrackerFrame.getUserMap();
const nite::Array<nite::UserData>& users = userTrackerFrame.getUsers();
float x,y,z;
x = y = z = 0;
skip++;
if (skip > 60000)
skip = 1;
if (users.getSize() > 0 && skipAmount > 0)
{
if (jointIdx < 500) {
jointIdx++;
}
else
jointIdx = 0;
//user 0
const nite::UserData& user = users[0];
sampleViewer->updateUserState(user, userTrackerFrame.getTimestamp());
if (user.isNew())
{
sampleViewer->m_pUserTracker->startSkeletonTracking(user.getId());
sampleViewer->m_pUserTracker->startPoseDetection(user.getId(), nite::POSE_CROSSED_HANDS);
}
else if (!user.isLost())
{
nite::SkeletonJoint jh = users[0].getSkeleton().getJoint(nite::JOINT_HEAD);
nite::SkeletonJoint jrh = users[0].getSkeleton().getJoint(nite::JOINT_RIGHT_HAND);
// HEAD
if (jh.getPositionConfidence() > 0.5f) {
x = jh.getPosition().x; y = jh.getPosition().y; z = jh.getPosition().z;
jointHistoryH[jointIdx] = jh;
if (initial || skip%skipAmount ==0) {
initialH_x = x;
initialH_z = z;
initial = false;
headFront = headBack = headLeft = headRight = false;
}
}
#define RESISTANCE 0.1f // Damping effect on velocity
float fThrust = 0.7f; // Acceleration rate due to thrusters (units/s*s)
float fSeconds = 1.0f;
float fSecondsRot = 0.002f;
int distance = 200;
if (startFly) {
m_3DCamera.m_vVelocity;
if (goingIn)
m_3DCamera.Rotate(m_3DCamera.GetRightAxis(), fSecondsRot * 1);
else
m_3DCamera.Rotate(m_3DCamera.GetRightAxis(), fSecondsRot * -1);
}
CVector vAccel(0.0f);
if (abs(z - initialH_z) > distance) {
startFly = true;
if (initialH_z > z) {
headFront = true;
goingIn = true;
vAccel += m_3DCamera.GetViewAxis() * fThrust;
}
else {
headBack = true;
goingIn = false;
vAccel += m_3DCamera.GetViewAxis() * -fThrust;
}
initialH_z = z;
m_3DCamera.Accelerate(vAccel, fSeconds, RESISTANCE);
CVector vPos = m_3DCamera.GetPosition();
float fMagnitude = vPos.Magnitude();
if(fMagnitude < m_fInnerRadius)
{
vPos *= (m_fInnerRadius * (1 + DELTA)) / fMagnitude;
m_3DCamera.SetPosition(CDoubleVector(vPos.x, vPos.y, vPos.z));
m_3DCamera.SetVelocity(-m_3DCamera.GetVelocity());
}
t *arg1;
arg1 = (t *)malloc(sizeof(t));
sprintf(arg1->wavFile, "media/space_chord_1.wav");
_beginthread( PlayWav, 0, (void*) arg1);
}
if (abs(x - initialH_x) > 100)
if (initialH_x > x)
headRight = true;
else
headLeft = true;
sprintf(szBuffer, "initialH_x: %.1f x:%.1f ", initialH_x, x);
m_fFont.Print(szBuffer);
// RIGHT HAND
if (jrh.getPositionConfidence() > 0.5f) {
x = jrh.getPosition().x; y = jrh.getPosition().y; z = jrh.getPosition().z;
jointHistoryRH[jointIdx] = jrh;
if (initial || skip%skipAmount ==0) {
initialRH_x = x;
initialRH_z = z;
initial = false;
handLeft = handRight = false;
}
}
if (abs(x - initialRH_x) < 100)
if (initialRH_x > x)
handRight = true;
else
handLeft = true;
}
}
//PlayWav(WHITE_WAVE_FILE);
m_fFont.SetPosition(0, 30);
sprintf(szBuffer, "initialH_z: %.1f z:%.1f ", initialH_z, z);
m_fFont.Print(szBuffer);
m_fFont.SetPosition(0, 45);
//sprintf(szBuffer, "Users: %d hf:%d hb:%d hl:%d hr:%d hal:%d har:%d skAmount: %d v: %.3f ", users.getSize(), headFront, headBack, headLeft, headRight, handLeft, handRight, skipAmount, m_3DCamera.m_vVelocity.Magnitude());
sprintf(szBuffer, "Users: %d hf:%d hb:%d v: %.3f ", users.getSize(), headFront, headBack, m_3DCamera.m_vVelocity.Magnitude());
m_fFont.Print(szBuffer);
m_fFont.SetPosition(0, 60);
// m_fFont.Print(g_ALError);
m_fFont.End();
glFlush();
}
