void loop(void)
{
use_basic();
glEnable(GL_DEPTH_TEST);
if(mm)
rotz(pview,-mx*50.0f*dt);
else if(rx!=0.0f)
rotz(pview,-rx*200.0f*dt);
else rotz(pview,-10.0f*dt);
basic_pview(pview);
update_model_vbo();
clear();
use_tex(area_tex);
draw_model_vbo(&area_mod);
use_text_pro();
glDisable(GL_DEPTH_TEST);
use_tex(font.t);
draw_text(text.len);
if(kp(SDL_SCANCODE_DELETE) || kp(SDL_SCANCODE_ESCAPE) || bp(0))
quit();
}
/** Create or adjust "rot" parameter for a component
* Assumed that name either equals "rotx", "roty" or "rotz" otherwise this
* method will not add/modify "rot" parameter
* @param comp :: Component
* @param name :: Parameter name
* @param deg :: Parameter value in degrees
* @param pDescription :: a pointer (may be NULL) to a string, containing
* parameter's
* description. If provided, the contents of the string is copied to the
* parameter's
* memory
*/
void ParameterMap::addRotationParam(const IComponent *comp,
const std::string &name, const double deg,
const std::string *const pDescription) {
Parameter_sptr paramRotX = get(comp, rotx());
Parameter_sptr paramRotY = get(comp, roty());
Parameter_sptr paramRotZ = get(comp, rotz());
double rotX, rotY, rotZ;
if (paramRotX)
rotX = paramRotX->value<double>();
else
rotX = 0.0;
if (paramRotY)
rotY = paramRotY->value<double>();
else
rotY = 0.0;
if (paramRotZ)
rotZ = paramRotZ->value<double>();
else
rotZ = 0.0;
// adjust rotation
Quat quat;
if (name.compare(rotx()) == 0) {
addDouble(comp, rotx(), deg);
quat = Quat(deg, V3D(1, 0, 0)) * Quat(rotY, V3D(0, 1, 0)) *
Quat(rotZ, V3D(0, 0, 1));
} else if (name.compare(roty()) == 0) {
addDouble(comp, roty(), deg);
quat = Quat(rotX, V3D(1, 0, 0)) * Quat(deg, V3D(0, 1, 0)) *
Quat(rotZ, V3D(0, 0, 1));
} else if (name.compare(rotz()) == 0) {
addDouble(comp, rotz(), deg);
quat = Quat(rotX, V3D(1, 0, 0)) * Quat(rotY, V3D(0, 1, 0)) *
Quat(deg, V3D(0, 0, 1));
} else {
g_log.warning()
<< "addRotationParam() called with unrecognized coordinate symbol: "
<< name;
return;
}
// clear the position cache
clearPositionSensitiveCaches();
// finally add or update "pos" parameter
addQuat(comp, rot(), quat, pDescription);
}
void ArcBall::setView( int view )
{
m_zoom = 1.0;
m_moveX = 0;
m_moveY = 0;
m_oldMoveX = 0;
m_oldMoveY = 0;
m_currentRot.setToIdentity();
m_lastRot.setToIdentity();
QQuaternion rotx( sqrt(0.5), 0, 0, sqrt(0.5) );
QQuaternion rot_x( -sqrt(0.5), 0, 0, sqrt(0.5) );
QQuaternion roty( 0, sqrt(0.5), 0, sqrt(0.5) );
QQuaternion rot_y( 0, -sqrt(0.5), 0, sqrt(0.5) );
QQuaternion rotz( 0, 0, sqrt(0.5), sqrt(0.5) );
if ( view == 2 )
{
m_currentRot.rotate( rotz );
m_currentRot.rotate( rotx );
m_currentRot.rotate( rotx );
}
if ( view == 3 )
{
m_currentRot.rotate( rot_x );
m_currentRot.rotate( rot_y );
}
}
//rotate
void rotate(float rx, float ry, float rz){
Matrix4f rotx, roty, rotz;
rotx = Matrix4f::Identity();
roty = Matrix4f::Identity();
rotz = Matrix4f::Identity();
float cosrx, sinrx, cosry, sinry, cosrz, sinrz;
cosrx = cosf(rx); sinrx = sinf(rx);
cosry = cosf(ry); sinry = sinf(ry);
cosrz = cosf(rz); sinrz = sinf(rz);
rotx(1,1) = cosrx; rotx(1,2) = -sinrx;
rotx(2,1) = sinrx; rotx(2,2) = cosrx;
roty(0,0) = cosry; roty(2,0) = -sinry;
roty(0,2) = sinry; roty(2,2) = cosry;
rotz(0,0) = cosrz; rotz(0,1) = -sinrz;
rotz(1,0) = sinrz; rotz(1,1) = cosrz;
model_view.top() *= (rotx * roty * rotz);
}
void TvlqrControl::InitializeState(const mav_pose_t *msg) {
initial_state_ = PoseMsgToStateEstimatorVector(msg);
// get the yaw from the initial state
double rpy[3];
bot_quat_to_roll_pitch_yaw(msg->orientation, rpy);
Mz_ = rotz(-rpy[2]);
t0_ = GetTimestampNow();
state_initialized_ = true;
}
Matrix4D RotateZ(float theta)
{
Matrix4D rotz(cos(theta), -sin(theta), 0, 0, sin(theta), cos(theta), 0, 0, 0, 0, 1,0,0,0,0,1);
return rotz;
}
void GsMat::rotz ( float radians, char fmt )
{
rotz ( sinf(radians), cosf(radians), fmt );
}
