void fpscont(Camera* cam) {
// Rip out position
//
float vec_pos[4];
float* mat_world = cam->mat_world;
memcpy(vec_pos, &mat_world[12], sizeof(vec_pos));
memset(&mat_world[12], 0, sizeof(float) * 3);
// Mouse-look
//
short center_x = display.getWidth()/2;
short center_y = display.getHeight()/2;
short delta_x = mouse.pos[0] - center_x;
short delta_y = mouse.pos[1] - center_y;
mouse.setPos(center_x, center_y);
float mat_rotx[16];
matRot(mat_rotx, delta_y * -0.001f, 0);
matMul(mat_world, mat_rotx, mat_world);
float mat_roty[16];
matRot(mat_roty, delta_x * -0.001f, 1);
matMul(mat_roty, mat_world, mat_world);
// Movement
//
char up_down = keyboard.keyDown('w') - keyboard.keyDown('s');
char right_left = keyboard.keyDown('d') - keyboard.keyDown('a');
float speed = 0.5;
float vec_vel[4];
vec_vel[0] = right_left * speed;
vec_vel[1] = 0;
vec_vel[2] = -up_down * speed;
vec_vel[3] = 1;
matMulVec(mat_world, vec_vel, vec_vel);
vecAdd(vec_pos, vec_vel, vec_pos);
vec_pos[3] = 1;
memcpy(cam->mat_view, cam->mat_world, sizeof(float) * 16);
memcpy(&cam->mat_world[12], vec_pos, sizeof(vec_pos));
cameraWorldToView(cam);
}
void CHydraHmdLatest::DebugRequest( const char * pchRequest, char * pchResponseBuffer, uint32_t unResponseBufferSize )
{
std::istringstream ss( pchRequest );
std::string strCmd;
ss >> strCmd;
if ( strCmd == "hydra:realign_coordinates" )
{
// hydra_monitor is calling us back with HMD tracking information so we can
// finish realigning our coordinate system to the HMD's
float m[3][3], v[3];
for ( int i = 0; i < 3; ++i )
{
for ( int j = 0; j < 3; ++j )
{
// Note the transpose, because sixenseMath::Matrix3 and vr::HmdMatrix34_t disagree on row/col major
ss >> m[j][i];
}
ss >> v[i];
}
sixenseMath::Matrix3 matRot( m );
sixenseMath::Vector3 matPos( v );
FinishRealignCoordinates( matRot, matPos );
}
void Transform::evalRotationScale(Quat& rotation, Vec3& scale, const Mat3& rotationScaleMatrix) {
const float ACCURACY_THREASHOLD = 0.00001f;
// Following technique taken from:
// http://callumhay.blogspot.com/2010/10/decomposing-affine-transforms.html
// Extract the rotation component - this is done using polar decompostion, where
// we successively average the matrix with its inverse transpose until there is
// no/a very small difference between successive averages
float norm;
int count = 0;
Mat3 rotationMat = rotationScaleMatrix;
do {
Mat3 currInvTranspose = glm::inverse(glm::transpose(rotationMat));
Mat3 nextRotation = 0.5f * (rotationMat + currInvTranspose);
norm = 0.0;
for (int i = 0; i < 3; i++) {
float n = static_cast<float>(
fabs(rotationMat[0][i] - nextRotation[0][i]) +
fabs(rotationMat[1][i] - nextRotation[1][i]) +
fabs(rotationMat[2][i] - nextRotation[2][i]));
norm = (norm > n ? norm : n);
}
rotationMat = nextRotation;
} while (count++ < 100 && norm > ACCURACY_THREASHOLD);
// extract scale of the matrix as the length of each axis
Mat3 scaleMat = glm::inverse(rotationMat) * rotationScaleMatrix;
scale = glm::max(Vec3(ACCURACY_THREASHOLD), Vec3(scaleMat[0][0], scaleMat[1][1], scaleMat[2][2]));
// Let's work on a local matrix containing rotation only
Mat3 matRot(
rotationScaleMatrix[0] / scale.x,
rotationScaleMatrix[1] / scale.y,
rotationScaleMatrix[2] / scale.z);
// Beware!!! needs to detect for the case there is a negative scale
// Based on the determinant sign we just can flip the scale sign of one component: we choose X axis
float determinant = glm::determinant(matRot);
if (determinant < 0.0f) {
scale.x = -scale.x;
matRot[0] *= -1.0f;
}
// Beware: even though the matRot is supposed to be normalized at that point,
// glm::quat_cast doesn't always return a normalized quaternion...
// rotation = glm::normalize(glm::quat_cast(matRot));
rotation = (glm::quat_cast(matRot));
}
void S3D_MASTER::Render( bool aIsRenderingJustNonTransparentObjects,
bool aIsRenderingJustTransparentObjects )
{
if( m_parser == NULL )
return;
double aVrmlunits_to_3Dunits = g_Parm_3D_Visu.m_BiuTo3Dunits * UNITS3D_TO_UNITSPCB;
glScalef( aVrmlunits_to_3Dunits, aVrmlunits_to_3Dunits, aVrmlunits_to_3Dunits );
glm::vec3 matScale( m_MatScale.x,
m_MatScale.y,
m_MatScale.z );
glm::vec3 matRot( m_MatRotation.x,
m_MatRotation.y,
m_MatRotation.z );
glm::vec3 matPos( m_MatPosition.x,
m_MatPosition.y,
m_MatPosition.z );
glTranslatef( matPos.x * SCALE_3D_CONV,
matPos.y * SCALE_3D_CONV,
matPos.z * SCALE_3D_CONV );
glRotatef( -matRot.z, 0.0f, 0.0f, 1.0f );
glRotatef( -matRot.y, 0.0f, 1.0f, 0.0f );
glRotatef( -matRot.x, 1.0f, 0.0f, 0.0f );
glScalef( matScale.x, matScale.y, matScale.z );
for( unsigned int idx = 0; idx < m_parser->childs.size(); idx++ )
{
m_parser->childs[idx]->openGL_RenderAllChilds( aIsRenderingJustNonTransparentObjects,
aIsRenderingJustTransparentObjects );
}
}
