Mat4 Mat4::Rotate(double angle, Vec4 &axis){ double zAngle = angle *axis.z; Mat4 zRot( cos(zAngle), -sin(zAngle), 0, 0, sin(zAngle), cos(zAngle), 0, 0, 0, 0, 1, 0, 0, 0, 0, 1 ); double yAngle = angle *axis.y; Mat4 yRot( cos(yAngle), 0, sin(yAngle), 0, 0, 1, 0, 0, -sin(yAngle), 0, cos(yAngle), 0, 0, 0, 0, 0 ); double xAngle = angle *axis.x; Mat4 xRot( 1, cos(xAngle), -sin(xAngle), 0, 0, 1, 0, 0, 0, sin(xAngle), cos(xAngle), 0, 0, 0, 0, 0 ); Mat4 ret = zRot*yRot*xRot; // CMat4 ret = CMat4_MatMult(xRot, mat); // ret = CMat4_MatMult(yRot, ret); // ret = CMat4_MatMult(zRot, ret); return ret; }
void PerspectiveCamera::mouseDragged(CameraScratch &scratch, QMouseEvent *event) { int xDiff = scratch.pickX - event->pos().x(); int yDiff = scratch.pickY - event->pos().y(); if (scratch.moveType == MoveType::ROTATING) { Vector3 origLook = eye() + lookDir(); setYRot(yRot() + xDiff * 0.5f); setUpRot(upRot() + yDiff * -0.5f); // move eye to look at original focal point (Maya style) Vector3 lookAway = lookDir() * -1; setCenter(origLook + lookAway); } else if (scratch.moveType == MoveType::PANNING) { float panScale = 0.05f; Vector3 mUp = scratch.origUp * -1.0f * yDiff * panScale; Vector3 mLeft = scratch.origLeft * -1.0f * xDiff * panScale; setCenter(eye() + mUp + mLeft); } else if (scratch.moveType == MoveType::TRUCKING) { Point3 at = lookat(); Vector3 l = lookDir() * -0.01f * yDiff; setCenter(l + center()); } scratch.pickX = event->pos().x(); scratch.pickY = event->pos().y(); }
//-------------------------------------------------------------- void ofApp::mouseDragged(int x, int y, int button){ //every time the mouse is dragged, track the change //accumulate the changes inside of curRot through multiplication ofVec2f mouse(x,y); ofQuaternion yRot((x-lastMouse.x)*dampen, ofVec3f(0,1,0)); ofQuaternion xRot((y-lastMouse.y)*dampen, ofVec3f(-1,0,0)); curRot *= yRot*xRot; lastMouse = mouse; }
/*locks the axis to the specified rotation*/ void Transform::LockAxisTo(Kiwi::AXIS axis, float radians) { switch(axis) { case Kiwi::AXIS::X_AXIS: { m_lockPitch = true; m_lockPosition.x = radians; Kiwi::Vector3 eulerAngles = m_rotation.GetEulerAngles(); if(eulerAngles.x != m_lockPosition.x) { Kiwi::Quaternion xRot(this->GetRight(),-(eulerAngles.x - m_lockPosition.x)); m_rotation = xRot.Cross(m_rotation); } break; } case Kiwi::AXIS::Y_AXIS: { m_lockYaw = true; m_lockPosition.y = radians; Kiwi::Vector3 eulerAngles = m_rotation.GetEulerAngles(); if(eulerAngles.y != m_lockPosition.y) { Kiwi::Quaternion yRot(this->GetUp(),-(eulerAngles.y - m_lockPosition.y)); m_rotation = yRot.Cross(m_rotation); } break; } case Kiwi::AXIS::Z_AXIS: { m_lockRoll = true; m_lockPosition.z = radians; Kiwi::Vector3 eulerAngles = m_rotation.GetEulerAngles(); if(eulerAngles.z != m_lockPosition.z) { Kiwi::Quaternion zRot(this->GetForward(),-(eulerAngles.z - m_lockPosition.z)); m_rotation = zRot.Cross(m_rotation); } break; } default: break; } }
osg::Quat FaceTransform::computeQuat(osg::Vec3d direction) { direction.normalize(); double zAngle = atan2(direction.y(), direction.x()); osg::Quat zRot(zAngle, osg::Vec3d(0,0,1)); osg::Vec3d yAxis = zRot * osg::Vec3d(0,1,0); double zLength = (direction - osg::Vec3d(0,0,direction.z())).length(); double yAngle = - atan2(direction.z(), zLength) + osg::PI/2; osg::Quat yRot(yAngle, yAxis); return zRot * yRot; }
void Transform::Rotate(const Kiwi::Quaternion& rotation) { //multiply the current rotation with the new rotation to get the final rotation m_rotation = rotation.Cross(m_rotation); //if an axis is locked, we need to rotate that axis back to its fixed rotation value if(m_lockRoll) { Kiwi::Vector3 eulerAngles = m_rotation.GetEulerAngles(); if(eulerAngles.z != m_lockPosition.z) { Kiwi::Quaternion zRot(this->GetForward(),-(eulerAngles.z - m_lockPosition.z)); m_rotation = zRot.Cross(m_rotation); } } if(m_lockPitch) { Kiwi::Vector3 eulerAngles = m_rotation.GetEulerAngles(); if(eulerAngles.x != m_lockPosition.x) { Kiwi::Quaternion xRot(this->GetRight(),-(eulerAngles.x - m_lockPosition.x)); m_rotation = xRot.Cross(m_rotation); } } if(m_lockYaw) { Kiwi::Vector3 eulerAngles = m_rotation.GetEulerAngles(); if(eulerAngles.y != m_lockPosition.y) { Kiwi::Quaternion yRot(this->GetUp(),-(eulerAngles.y - m_lockPosition.y)); m_rotation = yRot.Cross(m_rotation); } } }
void RCViewableTransform::rotateAboutPoint(const Eks::Vector3D &point, float x, float y) { if(_rotateEnabled) { Eks::Transform t = transform(); // old translation vector float length = (t.translation() - point).norm(); Eigen::AngleAxisf xRot(x * -0.005f, upVector()); t.prerotate(xRot); Eigen::AngleAxisf yRot(y * -0.005f, Eks::Vector3D(1.0f, 0.0f, 0.0f)); t.rotate(yRot); Eks::Vector3D newLook = t.matrix().col(2).head<3>(); t.translation() = point + (newLook * length); transform = t; } }
//-------------------------------------------------------------- void game::mouseDragged(int x, int y, int button){ if(step == 3){ ofVec2f mouseA(x,y); //look for difference in x /* if(mouseA.x > lastMouseA.x){ //moved right rotateSlicer.y += 1; }else if(mouseA.x < lastMouseA.x){ //moved left rotateSlicer.y -= 1; } //look for difference in y if(mouseA.y > lastMouseA.y){ //moved up rotateSlicer.x -= 1; }else if(mouseA.y < lastMouseA.y){ //moved down rotateSlicer.x += 1; } */ rotateSlicer.y += mouseA.x-lastMouseA.x; rotateSlicer.x -= mouseA.y-lastMouseA.y; lastMouseA = mouseA; }else if(step == 4 || step == 5){ ofVec2f mouse(x,y); ofQuaternion yRot(x-lastMouse.x, ofVec3f(0,1,0)); ofQuaternion xRot(y-lastMouse.y, ofVec3f(-1,0,0)); //curRot *= yRot*xRot; curRot.set(curRot*yRot*xRot); lastMouse = mouse; }else if(step == 6){ myCanvas->mouseDragged(x,y,button); } }
// Main application int main( int argc, char* argv[] ) { printf("== NaturalPoint Tracking Tools API Marker Sample =======---\n"); printf("== (C) NaturalPoint, Inc.\n\n"); printf("Initializing NaturalPoint Devices\n"); TT_Initialize(); // Do an update to pick up any recently-arrived cameras. TT_Update(); // Load a project file from the executable directory. printf( "Loading Project: project.ttp\n\n" ); CheckResult( TT_LoadProject("project.ttp") ); // List all detected cameras. printf( "Cameras:\n" ); for( int i = 0; i < TT_CameraCount(); i++) { printf( "\t%s\n", TT_CameraName(i) ); } printf("\n"); // List all defined rigid bodies. printf("Rigid Bodies:\n"); for( int i = 0; i < TT_TrackableCount(); i++) { printf("\t%s\n", TT_TrackableName(i)); } printf("\n"); int frameCounter = 0; // Poll API data until the user hits a keyboard key. while( !_kbhit() ) { if( TT_Update() == NPRESULT_SUCCESS ) { frameCounter++; // Update tracking information every 100 frames (for example purposes). if( (frameCounter%100) == 0 ) { float yaw,pitch,roll; float x,y,z; float qx,qy,qz,qw; bool tracked; printf( "Frame #%d: (Markers: %d)\n", frameCounter, TT_FrameMarkerCount() ); for( int i = 0; i < TT_TrackableCount(); i++ ) { TT_TrackableLocation( i, &x,&y,&z, &qx,&qy,&qz,&qw, &yaw,&pitch,&roll ); if( TT_IsTrackableTracked( i ) ) { printf( "\t%s: Pos (%.3f, %.3f, %.3f) Orient (%.1f, %.1f, %.1f)\n", TT_TrackableName( i ), x, y, z, yaw, pitch, roll ); TransformMatrix xRot( TransformMatrix::RotateX( -roll * kRadToDeg ) ); TransformMatrix yRot( TransformMatrix::RotateY( -yaw * kRadToDeg ) ); TransformMatrix zRot( TransformMatrix::RotateZ( -pitch * kRadToDeg ) ); // Compose the local-to-world rotation matrix in XZY (roll, pitch, yaw) order. TransformMatrix worldTransform = xRot * zRot * yRot; // Inject world-space coordinates of the origin. worldTransform.SetTranslation( x, y, z ); // Invert the transform matrix to convert from a local-to-world to a world-to-local. worldTransform.Invert(); float mx, my, mz; int markerCount = TT_TrackableMarkerCount( i ); for( int j = 0; j < markerCount; ++j ) { // Get the world-space coordinates of each rigid body marker. TT_TrackablePointCloudMarker( i, j, tracked, mx, my, mz ); // Transform the rigid body point from world coordinates to local rigid body coordinates. // Any world-space point can be substituted here to transform it into the local space of // the rigid body. Point4 worldPnt( mx, my, mz, 1.0f ); Point4 localPnt = worldTransform * worldPnt; printf( "\t\t%d: World (%.3f, %.3f, %.3f) Local (%.3f, %.3f, %.3f)\n", j + 1, mx, my, mz, localPnt[0], localPnt[1], localPnt[2] ); } } else { printf( "\t%s: Not Tracked\n", TT_TrackableName( i ) ); } } } } Sleep(2); } printf( "Shutting down NaturalPoint Tracking Tools\n" ); CheckResult( TT_Shutdown() ); printf( "Complete\n" ); while( !_kbhit() ) { Sleep(20); } TT_FinalCleanup(); return 0; }
/* static */ GfMatrix4d UsdGeomXformOp::GetOpTransform(UsdGeomXformOp::Type const opType, VtValue const &opVal, bool isInverseOp) { // This will be the most common case. if (opType == TypeTransform) { GfMatrix4d mat(1.); bool isMatrixVal = true; if (opVal.IsHolding<GfMatrix4d>()) { mat = opVal.UncheckedGet<GfMatrix4d>(); } else if (opVal.IsHolding<GfMatrix4f>()) { mat = GfMatrix4d(opVal.UncheckedGet<GfMatrix4f>()); } else { isMatrixVal = false; TF_CODING_ERROR("Invalid combination of opType (%s) " "and opVal (%s). Returning identity matrix.", TfEnum::GetName(opType).c_str(), TfStringify(opVal).c_str()); return GfMatrix4d(1.); } if (isMatrixVal && isInverseOp) { double determinant=0; mat = mat.GetInverse(&determinant); if (GfIsClose(determinant, 0.0, 1e-9)) { TF_CODING_ERROR("Cannot invert singular transform op with " "value %s.", TfStringify(opVal).c_str()); } } return mat; } double doubleVal = 0.; bool isScalarVal = true; if (opVal.IsHolding<double>()) { doubleVal = opVal.UncheckedGet<double>(); } else if (opVal.IsHolding<float>()) { doubleVal = opVal.UncheckedGet<float>(); } else if (opVal.IsHolding<GfHalf>()) { doubleVal = opVal.UncheckedGet<GfHalf>(); } else { isScalarVal = false; } if (isScalarVal) { if (isInverseOp) doubleVal = -doubleVal; if (opType == TypeRotateX) { return GfMatrix4d(1.).SetRotate(GfRotation(GfVec3d::XAxis(), doubleVal)); } else if (opType == TypeRotateY) { return GfMatrix4d(1.).SetRotate(GfRotation(GfVec3d::YAxis(), doubleVal)); } else if (opType == TypeRotateZ) { return GfMatrix4d(1.).SetRotate(GfRotation(GfVec3d::ZAxis(), doubleVal)); } } GfVec3d vec3dVal = GfVec3d(0.); bool isVecVal = true; if (opVal.IsHolding<GfVec3f>()) { vec3dVal = opVal.UncheckedGet<GfVec3f>(); } else if (opVal.IsHolding<GfVec3d>()) { vec3dVal = opVal.UncheckedGet<GfVec3d>(); } else if (opVal.IsHolding<GfVec3h>()) { vec3dVal = opVal.UncheckedGet<GfVec3h>(); } else { isVecVal = false; } if (isVecVal) { switch(opType) { case TypeTranslate: if (isInverseOp) vec3dVal = -vec3dVal; return GfMatrix4d(1.).SetTranslate(vec3dVal); case TypeScale: if (isInverseOp) { vec3dVal = GfVec3d(1/vec3dVal[0], 1/vec3dVal[1], 1/vec3dVal[2]); } return GfMatrix4d(1.).SetScale(vec3dVal); default: { if (isInverseOp) vec3dVal = -vec3dVal; // Must be one of the 3-axis rotates. GfMatrix3d xRot(GfRotation(GfVec3d::XAxis(), vec3dVal[0])); GfMatrix3d yRot(GfRotation(GfVec3d::YAxis(), vec3dVal[1])); GfMatrix3d zRot(GfRotation(GfVec3d::ZAxis(), vec3dVal[2])); GfMatrix3d rotationMat(1.); switch (opType) { case TypeRotateXYZ: // Inv(ABC) = Inv(C) * Inv(B) * Inv(A) rotationMat = !isInverseOp ? (xRot * yRot * zRot) : (zRot * yRot * xRot); break; case TypeRotateXZY: rotationMat = !isInverseOp ? (xRot * zRot * yRot) : (yRot * zRot * xRot); break; case TypeRotateYXZ: rotationMat = !isInverseOp ? (yRot * xRot * zRot) : (zRot * xRot * yRot); break; case TypeRotateYZX: rotationMat = !isInverseOp ? (yRot * zRot * xRot) : (xRot * zRot * yRot); break; case TypeRotateZXY: rotationMat = !isInverseOp ? (zRot * xRot * yRot) : (yRot * xRot * zRot); break; case TypeRotateZYX: rotationMat = !isInverseOp ? (zRot * yRot * xRot) : (xRot * yRot * zRot); break; default: TF_CODING_ERROR("Invalid combination of opType (%s) " "and opVal (%s). Returning identity matrix.", TfEnum::GetName(opType).c_str(), TfStringify(opVal).c_str()); return GfMatrix4d(1.); } return GfMatrix4d(1.).SetRotate(rotationMat); } } } if (opType == TypeOrient) { GfQuatd quatVal(0); if (opVal.IsHolding<GfQuatd>()) quatVal = opVal.UncheckedGet<GfQuatd>(); else if (opVal.IsHolding<GfQuatf>()) { const GfQuatf &quatf = opVal.UncheckedGet<GfQuatf>(); quatVal = GfQuatd(quatf.GetReal(), quatf.GetImaginary()); } else if (opVal.IsHolding<GfQuath>()) { const GfQuath &quath = opVal.UncheckedGet<GfQuath>(); quatVal = GfQuatd(quath.GetReal(), quath.GetImaginary()); } GfRotation quatRotation(quatVal); if (isInverseOp) quatRotation = quatRotation.GetInverse(); return GfMatrix4d(quatRotation, GfVec3d(0.)); } TF_CODING_ERROR("Invalid combination of opType (%s) and opVal (%s). " "Returning identity matrix.", TfEnum::GetName(opType).c_str(), TfStringify(opVal).c_str()); return GfMatrix4d(1.); }