Camera::Camera(glm::vec3 position, glm::vec3 direction) {
//Create the camera direction and position
this->direction = direction;
this->position = position;
//Create the mouse x and y
xPos = 0;
yPos = 0;
//Create the viewing angles
EulerAngle angles = EulerAngle();
angles.toAngles(this->direction - this->position);
this->pitch = angles.getPitch();
this->yaw = angles.getYaw();
this->roll = 0;
fprintf(stdout, "Pitch: %f, Yaw: %f", pitch, yaw);
//Set the speed variables
speed = 3;
sensitivity = 0.02;
//Create the projection variables
fov = 45.0;
minDistance = 0.1;
maxDistance = 100.0;
}
void SceneObjectManipulator::resetAll()
{
m_vecTranslation = QVector3D(0,0,0);
m_vecScale = QVector3D(1,1,1);
m_angAngles = EulerAngle(0,0,0);
updateObjectSoft();
}
void SceneObjectManipulator::apply()
{
updateObjectSoft();
updateCachedProperties();
m_vecTranslation = QVector3D(0,0,0);
m_vecScale = QVector3D(1,1,1);
m_angAngles = EulerAngle(0,0,0);
}
void Virtual3dCharacterViewer::mouseMoveEvent(QMouseEvent * e)
{
if (RightButtonPressed && !isComport)
{
if ( JointSelected )
{
Vector3D newPosInCamera = getHomogeneous ( QPointToVector2D(e->pos()), SelectedJointPosInCamera.getZ() ) ;
Vector3D newPosInWorld = TransformVector(WorldMatInv, newPosInCamera) ;
*pSelectedJointPosRaw = newPosInWorld ;
triggerIK () ;
jointMoved () ;
}
}
else if ( LeftButtonPressed )
{
QPoint p = e->pos() ;
QPoint p2 = LeftButtonPressPos ;
float dx, dy;
dx = p.x() - p2.x() ;
dy = p.y() - p2.y() ;
dy /= height() ;
dx /= height() ; // get the ratio based on height .
dy = -dy ; // 屏幕坐标与空间坐标的y轴相反
cQuaternion Q1, dQ, Q2 ;
float rTod = 180 / PI ;
float dTor = PI / 180 ;
cEulerToQuat ( WorldInCameraBackup.Ori.getRoll()*dTor, WorldInCameraBackup.Ori.getPitch()*dTor, WorldInCameraBackup.Ori.getYaw()*dTor, &Q1) ;
float temp = Q1.y ;
Q1.y = Q1.z ;
Q1.z = Q1.x ;
Q1.x = temp ;
Vector3D v1 ;
Vector3D v2 ;
Vector3D v3 ;
v1 = Vector3D(dx, dy, 0) ;
v2 = Vector3D(0,0,1) ;
v3 = Vector3D::Cross(v2,v1).normalize() ;
float angle = v1.getLength()*PI/2;
dQ.w = cos(angle/2) ;
dQ.x = sin(angle/2) * v3.getX() ;
dQ.y = sin(angle/2) * v3.getY() ;
dQ.z = sin(angle/2) * v3.getZ() ;
cMultQuat ( &dQ, &Q1, &Q2 ) ;
temp = Q2.x ;
Q2.x = Q2.z ;
Q2.z = Q2.y ;
Q2.y = temp ;
cEulerAngle eu ;
cQuatToEuler ( &Q2 , &eu ) ; //
WorldInCamera.Ori = EulerAngle(eu.yaw, eu.pitch, eu.roll) ;
WorldInCamera.Ori = WorldInCamera.Ori.RadToDegree() ;
}
}
void SimpleCamera::Update(float deltaTime)
{
if(Keyboard::getKeyPressed(KEY_D)) {
rotate(-m_speed * deltaTime);
}
if (Keyboard::getKeyPressed(KEY_W)) {
rotateUp -= m_speed * deltaTime;
}
if (Keyboard::getKeyPressed(KEY_S)) {
rotateUp += m_speed * deltaTime;
}
if (Keyboard::getKeyPressed(KEY_A)) {
rotate(m_speed * deltaTime);
}
if(Keyboard::getKeyPressed(KEY_Q))
{
distance -= m_speed * deltaTime;
}
if (Keyboard::getKeyPressed(KEY_E))
{
distance += m_speed * deltaTime;
}
if(m_focus!=nullptr){
Vector3 focusPos = m_focus->getPositionVec();
Vector3 eulerRotVec = EulerAngle(rotationAngle, rotateUp, 0).toVector3();
forwardVec = Vector3::Normalize(focusPos + eulerRotVec);
eye = focusPos;
eye += eulerRotVec * distance;
m_lookAt = focusPos;
upVec = Vector3::up();
}
else
{
forwardVec = EulerAngle(rotationAngle, rotateUp, 0).toVector3();
m_lookAt = Vector3::zero() - forwardVec;
eye = m_lookAt * distanceSun;
upVec = Vector3::Normalize(forwardVec - Vector3(0, sin(180), 0));
}
Camera::Update(deltaTime);
}
void CCoordinateToolPhysicsObject::GetParentTransform( NxMat34& transform )
{
Matrix4x4 parentTrans;
static DWORD msgHash_GetGlobalTransform = CHashString(_T("GetGlobalTransform")).GetUniqueID();
m_ToolBox->SendMessage( msgHash_GetGlobalTransform, sizeof(parentTrans), &parentTrans, GetParentName(), &m_hsParentType );
// remove scale and rotation
parentTrans.SetRotation( EulerAngle() );
transform.setColumnMajor44( parentTrans.GetMatrix() );
}
//Calculate the view matrix from the mouse positions
void Camera::calculateViewMatrix(Window window) {
//Get the mouse positions
xPos, yPos;
glfwGetCursorPos(window.getWindow(), &xPos, &yPos);
//Reset the mouse position
int width, height;
glfwGetWindowSize(window.getWindow(), &width, &height);
glfwSetCursorPos(window.getWindow(), width / 2, height / 2);
//Calculate the change in the x and y position
double deltaXPos = xPos - (width / 2);
double deltaYPos = yPos - (height / 2);
//Calculate the pitch and yaw values from the change in x and y
pitch += deltaYPos * sensitivity;
yaw += deltaXPos * sensitivity;
//Create the EulerAngles object to convert the angles into a direction vector
EulerAngle angles = EulerAngle(yaw, pitch, 0);
//Check the pitch and yaw values and stop them from inversing
angles.constrain();
//Get the direction vector from the angles
direction = angles.toVector();
//Movement speed and perpendicular vector to direction vector
glm::vec3 speed = glm::vec3(0.1, 0.1, 0.1);
glm::vec3 right = glm::cross(direction, glm::vec3(0, 1, 0));
//Calculate the position and movement
if (glfwGetKey(window.getWindow(), GLFW_KEY_W) == GLFW_PRESS) {
position -= (direction * speed);
}
if (glfwGetKey(window.getWindow(), GLFW_KEY_S) == GLFW_PRESS) {
position += (direction * speed);
}
if (glfwGetKey(window.getWindow(), GLFW_KEY_D) == GLFW_PRESS) {
position -= (right * speed);
}
if (glfwGetKey(window.getWindow(), GLFW_KEY_A) == GLFW_PRESS) {
position += (right * speed);
}
//Create the view matrix from the position and direction
view = glm::lookAt(
position,
position - direction,
glm::vec3(0, 1, 0)
);
}
FlightDynamics::FlightDynamics(const char* name, uint32_t stackSize, uint8_t priority, uint32_t eeprom_size)
: ApplicationModule(name, stackSize, priority, eeprom_size),
attitude_quaternion(EulerAngle(0,0,0))
{
//Subscribe relevant messages:
messenger.subscribe(CALIBRATE_GYROSCOPE);
messenger.subscribe(CALIBRATE_ACCELEROMETER);
messenger.subscribe(REQUEST_FLIGHTDYNAMICS_REPORT);
//Setup IIR filters for gyroscope readings:
pitch_filter.set_coeffs({0.0018, 0.0071, 0.0107, 0.0071, 0.0018, -2.7737, 3.019, -1.5048, 0.2879});
roll_filter.set_coeffs({0.0018, 0.0071, 0.0107, 0.0071, 0.0018, -2.7737, 3.019, -1.5048, 0.2879});
yaw_filter.set_coeffs({0.0018, 0.0071, 0.0107, 0.0071, 0.0018, -2.7737, 3.019, -1.5048, 0.2879});
//Reset the current attitude estimate:
attitude_socket.reset();
//Set the thread frequency to 400hz:
set_frequency(400);
}
Virtual3dCharacterViewer::Virtual3dCharacterViewer( QWidget* parent, bool fs , bool keyLimited )
: QGLWidget( parent )
{
isComport = false ;
int i ;
for ( i=0; i<sizeof(MarkCubePOT)/sizeof(MarkCubePOT[0]); i++ )
{
MarkCubePOT[i].Color = Vector3D(1,1,1) ;
MarkCubePOT[i].show = false ;
MarkCubePOT[i].Ori = EulerAngle(0,0,0) ;
MarkCubePOT[i].Pos = Vector3D(0,0,0) ;
}
for ( i=0; i<sizeof(MarkCubeFK)/sizeof(MarkCubeFK[0]); i++ )
{
MarkCubeFK[i].Color = Vector3D(0,1,0) ;
MarkCubeFK[i].show = false ;
MarkCubeFK[i].Ori = EulerAngle(0,0,0) ;
MarkCubeFK[i].Pos = Vector3D(0,0,0) ;
}
for ( i=0; i<sizeof(MarkCubeFKResult)/sizeof(MarkCubeFKResult[0]); i++ )
{
MarkCubeFKResult[i].Color = Vector3D(0,0,1) ;
MarkCubeFKResult[i].show = false ;
MarkCubeFKResult[i].Ori = EulerAngle(0,0,0) ;
MarkCubeFKResult[i].Pos = Vector3D(0,0,0) ;
}
for ( i=0; i<sizeof(MarkCubeDebugLeftArm)/sizeof(MarkCubeDebugLeftArm[0]); i++ )
{
MarkCubeDebugLeftArm[i].Color = Vector3D(1,0,1) ;
MarkCubeDebugLeftArm[i].show = false ;
MarkCubeDebugLeftArm[i].Ori = EulerAngle(0,0,0) ;
MarkCubeDebugLeftArm[i].Pos = Vector3D(0,0,0) ;
}
MarkCubeFK[0].show = true ;
MarkCubeFK[0].Pos = Vector3D(1,0,0) ;
MarkCubeFK[0].Ori = EulerAngle(30,50,78) ;
showFK();
showIK();
RightButtonPressed = LeftButtonPressed = false ;
JointSelected = false ;
initDefaultJointsDistance() ;
initVirtualBody() ;
IKResult.parent = this ;
FKResult.parent = this ;
WorldInCamera.Ori = EulerAngle(0,0,0) ;
WorldInCamera.Pos.setValues( 0,0,-2) ;
// new added .
HumanIK::SkeletonModel m ;
//humanIK.setModel( HumanIK::setDefaultModel(m), 1, IKResult.HipPos );
humanIK.setModel( HumanIK::setDefaultModel(m) );
filter = 0;
light = false ; //在initializeGL()中会使能光照
fullscreen = fs;
setWindowTitle("Quadcopter Emulator Widget v1.0 ---- Designed by NewThinker_Jiwey" );
if ( ! parent )
{
setGeometry( 120, 120, 640, 480 );
setMinimumSize(600,400);
}
else
{
setGeometry( 0, 0, parent->width(), parent->height() );
// setMinimumSize(600,400);
}
if ( fullscreen )
showFullScreen();
scanTimer.setInterval(15);
connect(&scanTimer,SIGNAL(timeout()),this,SLOT(show3D()));
scanTimer.start();
}
void SceneObjectManipulator::resetAngles()
{
m_angAngles = EulerAngle(0,0,0);
updateObjectSoft();
}
