void Camera::rotateView(float x, float y)
{
float thetaX = -y*right.x + x*up.x;
float thetaY = -y*right.y + x*up.y;
float thetaZ = -y*right.z + x*up.z;
if(cameraMode == FREEROAM_CAMERA)
{
vec3 newDir = vec3(rotZ(thetaZ)*rotX(thetaX)*rotY(thetaY) * vec4(dir, 1));
changeDir(newDir);
}
else if(cameraMode == MODELVIEWER_CAMERA)
{
pos = vec3(rotZ(thetaZ)*rotX(thetaX)*rotY(thetaY) * vec4(pos - viewCenter, 1)) + viewCenter; //Rotate position around center
float pos_length = length(pos-viewCenter);
vec3 _dir = normalize(pos-viewCenter);
/*if (abs(_dir.y) > maxY)
{
if (_dir.y < 0)
_dir.y = -maxY;
else
_dir.y = maxY;
float s = sqrt((1 - maxY*maxY) / (_dir.x*_dir.x + _dir.z*_dir.z));
_dir.x *= s;
_dir.z *= s;
_dir = normalize(_dir);
pos = viewCenter+_dir*pos_length;
}*/
if ((_dir.y > maxY) || (_dir.y < minY))
{
float s = 1.f;
if (_dir.y > maxY)
{
_dir.y = maxY;
s = sqrt((1 - maxY*maxY) / (_dir.x*_dir.x + _dir.z*_dir.z));
}
else if (_dir.y < minY)
{
_dir.y = minY;
s = sqrt((1 - minY*minY) / (_dir.x*_dir.x + _dir.z*_dir.z));
}
dir.x *= s;
_dir.z *= s;
_dir = normalize(_dir);
pos = viewCenter + _dir*pos_length;
}
changeDir(normalize(viewCenter-pos));
}
}
void COLLADA_Viewer::SixAxis(float flX, float flY, float flZ, float flG)
{
(void) flG;
if (_CrtRender.UsingPhysics())
{
static float dampedX = 0.0f;
static float dampedY = 0.0f;
static float dampedZ = 0.0f;
dampedX=(flX + 10.f*dampedX)/11.f;
dampedY=(flY + 10.f*dampedY)/11.f;
dampedZ=(flZ + 10.f*dampedZ)/11.f;
btTransform floorTrans;
floorTrans.setIdentity();
btVector3 xAxis(1,0,0);
btVector3 zAxis(0,0,1);
btQuaternion rotX(zAxis,dampedX*10.f);
btQuaternion rotZ(xAxis,-dampedZ*10.f);
btQuaternion combinedRot = rotZ*rotX;
btMatrix3x3 orn;
orn.setRotation(combinedRot);
btVector3 up = orn.getColumn(1);
CrtVec3f newG = CrtVec3f(up.getX(),up.getY(),up.getZ());
_CrtRender.SetGravity(newG);
// printf("new gravity (%f, %f, %f)\n", newG.getX(), newG.getY(), newG.getZ());
}
}
void OpekeTool::rotateZ()
{
int temp = sizeX->value();
sizeX->setValue(sizeY->value());
sizeY->setValue(temp);
emit rotZ();
}
void Camera::setZRotation(const float angle)
{
float sourceRot[16], rotZMat[16];
memcpy_s(sourceRot,sizeof(float) * 16,m_rotMatrix,sizeof(float) * 16);
rotZ( rad2deg(m_zMov),rotZMat);
multMat(rotZMat, sourceRot, m_rotMatrix);
}
void NIFTiROSOgreUtil::convertOrientationFromROSToOgre(Ogre::Quaternion& orientation)
{
// http://www.ogre3d.org/tikiwiki/tiki-index.php?page=Quaternion+and+Rotation+Primer
// Rotates the quaternion 90 deg along the x axis and -90 deg along the z axis
Ogre::Quaternion rotXNeg(-sqrt(0.5), -sqrt(0.5), 0, 0);
Ogre::Quaternion rotZ(-sqrt(0.5), 0, 0, sqrt(0.5));
orientation = orientation * rotZ * rotXNeg;
}
void Camera::rotateViewAround(float x, float y)
{
float thetaX = -y*right.x + x*up.x;
float thetaY = -y*right.y + x*up.y;
float thetaZ = -y*right.z + x*up.z;
pos = vec3(rotZ(thetaZ)*rotX(thetaX)*rotY(thetaY)* vec4(pos - viewCenter, 1) )+ viewCenter; //Rotate position around center
changeDir(viewCenter-pos);
}
void OpekeTool::setupActions()
{
connect (b_rotateX, SIGNAL(clicked()), this, SIGNAL(rotX()));
connect (b_rotateY, SIGNAL(clicked()), this, SIGNAL(rotY()));
connect (b_rotateZ, SIGNAL(clicked()), this, SIGNAL(rotZ()));
connect ( color, SIGNAL ( highlighted ( QColor ) ), this, SIGNAL ( colorChanged( QColor ) ) );
connect ( color, SIGNAL ( activated ( QColor ) ), this, SIGNAL ( colorViewed ( QColor ) ) );
connect ( planeZ, SIGNAL ( valueChanged ( int ) ), this, SIGNAL ( planeChanged ( int ) ) );
connect ( sizeX, SIGNAL ( valueChanged ( int ) ), this, SIGNAL ( sizeXChanged ( int ) ) );
connect ( sizeY, SIGNAL ( valueChanged ( int ) ), this, SIGNAL ( sizeYChanged ( int ) ) );
connect ( sizeZ, SIGNAL ( valueChanged ( int ) ), this, SIGNAL ( sizeZChanged ( int ) ) );
}
/* Returns a 3x3 rotation matrix.
* Starting at the base of the chain (links_[0]) this
* walks and composes module's rotations together to get
* the rotation matrix that brings the i-th module from
* its module frame to the world frame.
*
* TODO: Write another version of this that fills in a
* 3 x 3N matrix with all the rotation matricies for
* the current chain configuration.
* Then move this outside of the loops in tip_base_step
* and base_tip_step.
*/
Eigen::Matrix3d
ckbot::chain::get_current_R(int i)
{
Eigen::Matrix3d R;
R = links_[0].get_init_rotation();
/* Each module's joint angle is wrt the joint toward link
* and each module's R_jts is the rotation matrix that
* takes the vector of its base joint (the joint at which
* its joint angle is measured) to its tip joint
*
* This explains the ordering of rotation matrix multiplications here.
*/
for (int j = 0; j <= i; ++j)
{
R *= rotZ(links_[j].get_q())*links_[j].get_R_jts();
}
return R;
}
void SphereObject::transform()
{
//Create transformation matrices
Matrix scale(SCALE, xScale, yScale, zScale);
Matrix rotX(ROT_X, xAngle * MATH_PI / 180.0f);
Matrix rotY(ROT_Y, yAngle * MATH_PI / 180.0f);
Matrix rotZ(ROT_Z, zAngle * MATH_PI / 180.0f);
Matrix axisRot(IDENTITY);
axisRot.set(ROT_AXIS_ANGLE, this -> rotAxis, this -> axisRotAngle * MATH_PI / 180.0f);
Matrix orbitRot(IDENTITY);
orbitRot.set(ROT_AXIS_ANGLE, this -> orbitAxis, this -> orbitAngle * MATH_PI / 180.0f);
Matrix trans( TRANS, this -> startPos[x] + this -> xTrans, this -> startPos[y] + this -> yTrans, this -> startPos[z] + this -> zTrans );
Matrix orbitTrans(TRANS, this -> xOrbitTrans, this -> yOrbitTrans, this -> zOrbitTrans );
//Create the normal matrix
this -> normal = rotX * rotY * rotZ * axisRot * (orbitTrans * orbitRot);
//Create the local to world matrix (ie Model)
this -> world = scale * rotX * rotY * rotZ * axisRot * (orbitTrans * orbitRot) * trans;
// Create the ModelView ( LocalToWorld * View)
// Some pipelines have the project concatenated, others don't
// Best to keep the separated, you can always join them with a quick multiply
CameraManager* cm = CameraManager::getInstance();
this -> modelView = this -> world * cm -> getCam1() -> getViewMatrix();
//Since we are using the supplied flat shader we need to send it the
//combined modelView and Projection Matrices
this -> modelViewProj = this -> modelView * cm -> getCam1() -> getProjMatrix();
//Transform the lighting position
this -> transformedLightPos = this -> lightPos;// * cm -> getCam1() -> getViewMatrix(); Looks like I do not need this for now
}
bool Skeleton::setBoneRotationsAngles(const unsigned int& idx, const vec3& angles)
{
if(idx >= mBones.size())
{
return false;
}
// rotation around x
anax rotX(angles.x(), vec3::UnitX()); // pitch
anax rotY(angles.y(), vec3::UnitY()); // yaw
anax rotZ(angles.z(), vec3::UnitZ()); // roll
quat q = rotX * rotY * rotZ;
mBones[idx].R = q.matrix();
std::vector<REAL> len;
getBoneLengths(len);
fitToBoneLengths(len);
return true;
}
/* Walks along the chain from base to the i-th module
* adding each module's simple angular velocity (transformed to
* the world frame) and then returning the compound angular velocity
* of the i-th module that results.
*/
Eigen::Vector3d
ckbot::chain::get_angular_velocity(int i)
{
Eigen::Vector3d omega(0,0,0);
Eigen::Matrix3d R;
R = links_[0].get_init_rotation();
Eigen::Vector3d tmp3vec(0,0,0);
for (int cur=0; cur <= i; ++cur)
{
/* 6DOF Update needed */
tmp3vec << 0,0, links_[cur].get_qd();
omega += R*tmp3vec;
/* Updating R happens after updating omega, because each
* module rotates about its base joint which does
* not depend on this module's joint angle.
*/
R = R*rotZ(links_[cur].get_q())*links_[cur].get_R_jts();
}
return omega;
}
mat3<float> rotationMat3(float a, vec3<float> axis)
{
a *= DEGTORAD;
double cosAx = cos(a * axis.x);
double sinAx = sin(a * axis.x);
double cosAy = cos(a * axis.y);
double sinAy = sin(a * axis.y);
double cosAz = cos(a * axis.z);
double sinAz = sin(a * axis.z);
mat3<float> rotX(1, 0, 0,
0, cosAx,-sinAx,
0, sinAx, cosAx);
mat3<float> rotY(cosAy, 0, sinAy,
0, 1, 0,
-sinAy, 0, cosAy);
mat3<float> rotZ(cosAz,-sinAz, 0,
sinAz, cosAz, 0,
0, 0, 1);
return rotX * rotY * rotZ;
}
void CTwisterDialog::UpdateRotation(CString & strResult)
{
bool error = FALSE;
double x, y, z;
x=y=z=0;
x = atof(m_strX);
y = atof(m_strY);
z = atof(m_strZ);
if(!m_bRadians) {
x *= V_DEGTORAD;
y *= V_DEGTORAD;
z *= V_DEGTORAD;
}
CAxisRotation rotX(1, 0, 0, x);
CAxisRotation rotY(0, 1, 0, y);
CAxisRotation rotZ(0, 0, 1, z);
CQuaternion quatX(rotX);
CQuaternion quatY(rotY);
CQuaternion quatZ(rotZ);
CQuaternion quatResult = quatZ * quatY * quatX;
CAxisRotation rotResult(quatResult);
strResult = rotResult.ToString(2);
return;
}
void Opeke::setupActions()
{
KStandardAction::openNew ( this, SLOT ( fileNew() ), actionCollection() );
KStandardAction::quit ( qApp, SLOT ( quit() ), actionCollection() );
KStandardAction::save ( this, SLOT ( saveFile() ), actionCollection() );
KStandardAction::open ( this, SLOT ( openFile() ), actionCollection() );
KStandardAction::saveAs ( this, SLOT ( saveFileAs() ), actionCollection() );
KStandardAction::preferences ( this, SLOT ( optionsPreferences() ), actionCollection() );
undoAct = KStandardAction::undo (m_view, SLOT(undo()), actionCollection() );
redoAct = KStandardAction::redo (m_view, SLOT(redo()), actionCollection() );
removeAct = KStandardAction::cut (m_view, SLOT(delBrick()), actionCollection());
removeAct->setShortcut(QKeySequence(Qt::Key_Delete));
removeAct->setText( i18n ("Delete"));
removeAct->setEnabled(false);
// The action to start building Bricks in OGRE
KAction *build = new KAction ( KIcon ( "build" ), i18n ( "Build" ), this );
build->setShortcut(QKeySequence(Qt::Key_B));
actionCollection()->addAction ( QLatin1String ( "build_action" ), build );
connect ( build, SIGNAL ( triggered ( bool ) ), m_view, SLOT ( setBuildMode() ) );
// Start select mode
KAction *select = new KAction ( KIcon ( "select" ), i18n ( "S&elect" ), this );
select->setShortcut(QKeySequence(Qt::Key_E));
actionCollection()->addAction ( QLatin1String ( "select_action" ), select );
connect ( select, SIGNAL ( triggered ( bool ) ), m_view, SLOT ( setSelectMode() ) );
// Enable or disable grid
KAction *grid = new KAction (KIcon ( "grid" ), i18n ("Toggle &Grid"), this);
grid->setShortcut(QKeySequence(Qt::Key_G));
actionCollection()->addAction(QLatin1String("grid_action"), grid);
connect (grid, SIGNAL (triggered (bool)), m_view, SLOT (flipGridEnabled() ));
// Actions to choose what kind of Brick to build
KAction *block = new KAction (KIcon ( "block" ), i18n ("Build B&lock"), this);
block->setShortcut(QKeySequence(Qt::Key_L));
actionCollection()->addAction(QLatin1String("block_action"), block);
connect (block, SIGNAL (triggered (bool)), m_view, SLOT (changeTypeBlock() ));
connect (block, SIGNAL (triggered (bool)), m_view, SLOT (setBuildMode() ));
KAction *roof = new KAction (KIcon ( "roof" ), i18n ("Build &Roof"), this);
roof->setShortcut(QKeySequence(Qt::Key_R));
actionCollection()->addAction(QLatin1String("roof_action"), roof);
connect (roof, SIGNAL (triggered (bool)), m_view, SLOT (changeTypeRoof()));
connect (roof, SIGNAL (triggered (bool)), m_view, SLOT (setBuildMode() ));
KAction *cylinder = new KAction (KIcon ( "cylinder" ), i18n ("Build &Cylinder"), this);
cylinder->setShortcut(QKeySequence(Qt::Key_C));
actionCollection()->addAction(QLatin1String("cylinder_action"), cylinder);
connect (cylinder, SIGNAL (triggered (bool)), m_view, SLOT (changeTypeCylinder() ));
connect (cylinder, SIGNAL (triggered (bool)), m_view, SLOT (setBuildMode() ));
KAction *invCyl = new KAction (KIcon ("invCyl"), i18n ("Build &Inverted Cylinder"), this);
invCyl->setShortcut(QKeySequence(Qt::Key_I));
actionCollection()->addAction(QLatin1String("invCyl_action"), invCyl);
connect (invCyl, SIGNAL (triggered (bool)), m_view, SLOT (changeTypeInvCyl() ));
connect (invCyl, SIGNAL (triggered (bool)), m_view, SLOT (setBuildMode() ));
KAction *sphere = new KAction (KIcon ("sphere"), i18n ("Build &Sphere"), this);
sphere->setShortcut(QKeySequence(Qt::Key_S));
actionCollection()->addAction(QLatin1String("sphere_action"), sphere);
connect (sphere, SIGNAL (triggered (bool)), m_view, SLOT (changeTypeSphere() ));
connect (sphere, SIGNAL (triggered (bool)), m_view, SLOT (setBuildMode() ));
KAction *cone = new KAction (KIcon ( "cone" ), i18n ("Build Co&ne"), this);
cone->setShortcut(QKeySequence(Qt::Key_N));
actionCollection()->addAction(QLatin1String("cone_action"), cone);
connect (cone, SIGNAL (triggered (bool)), m_view, SLOT (changeTypeCone() ));
connect (cone, SIGNAL (triggered (bool)), m_view, SLOT (setBuildMode() ));
KAction *corner = new KAction (KIcon ( "corner" ), i18n ("Build Roof C&orner"), this);
corner->setShortcut(QKeySequence(Qt::Key_O));
actionCollection()->addAction(QLatin1String("corner_action"), corner);
connect (corner, SIGNAL (triggered (bool)), m_view, SLOT (changeTypeCorner() ));
connect (corner, SIGNAL (triggered (bool)), m_view, SLOT (setBuildMode() ));
KAction *invrcor = new KAction (KIcon ( "invrcor" ), i18n ("Build In&verted Roof Corner"), this);
invrcor->setShortcut(QKeySequence(Qt::Key_V));
actionCollection()->addAction(QLatin1String("invrcor_action"), invrcor);
connect (invrcor, SIGNAL (triggered (bool)), m_view, SLOT (changeTypeInvCorner() ));
connect (invrcor, SIGNAL (triggered (bool)), m_view, SLOT (setBuildMode() ));
KAction *pyramid = new KAction (KIcon ( "pyramid" ), i18n ("Build &Pyramid"), this);
pyramid->setShortcut(QKeySequence(Qt::Key_P));
actionCollection()->addAction(QLatin1String("pyramid_action"), pyramid);
connect (pyramid, SIGNAL (triggered (bool)), m_view, SLOT (changeTypePyramid() ));
connect (pyramid, SIGNAL (triggered (bool)), m_view, SLOT (setBuildMode() ));
KAction *snapshot = new KAction (KIcon ("ksnapshot"), i18n ("&Take Screenshot"), this);
snapshot->setShortcut(QKeySequence("Ctrl+T"));
actionCollection()->addAction(QLatin1String("snapshot_action"), snapshot);
connect (snapshot, SIGNAL(triggered(bool)), this, SLOT (saveScreen()));
// Reload the field after loading a new file
connect ( this, SIGNAL ( reload() ), m_view, SLOT ( update() ) );
connect ( this, SIGNAL ( clear() ), m_view, SLOT ( newScene() ) );
connect ( m_tool, SIGNAL ( colorViewed ( QColor ) ), m_view, SLOT ( viewColor ( QColor ) ) );
connect ( m_tool, SIGNAL ( colorChanged ( QColor ) ), m_view, SLOT ( setColor ( QColor ) ) );
connect ( m_tool, SIGNAL ( planeChanged ( int ) ), m_view, SLOT ( setPlaneZ ( int ) ) );
connect ( m_tool, SIGNAL ( sizeXChanged ( int ) ), m_view, SLOT ( setSizeX ( int ) ) );
connect ( m_tool, SIGNAL ( sizeYChanged ( int ) ), m_view, SLOT ( setSizeY ( int ) ) );
connect ( m_tool, SIGNAL ( sizeZChanged ( int ) ), m_view, SLOT ( setSizeZ ( int ) ) );
connect ( m_view, SIGNAL(planeChanged(int)), m_tool, SLOT(changePlaneZ(int)));
connect ( m_view, SIGNAL(undoEmpty(bool)), this, SLOT(undoEnable(bool)));
connect ( m_view, SIGNAL(redoEmpty(bool)), this, SLOT(redoEnable(bool)));
connect ( m_view, SIGNAL(delEnable(bool)), this, SLOT(removeEnable(bool)));
connect ( m_view, SIGNAL(modified()), this, SLOT(fileModified()));
connect (m_tool, SIGNAL(rotX()), m_view, SLOT(rotateX()));
connect (m_tool, SIGNAL(rotY()), m_view, SLOT(rotateY()));
connect (m_tool, SIGNAL(rotZ()), m_view, SLOT(rotateZ()));
}