void RotateHandle::renderRing(Model::RotateHandleHit* hit, Renderer::Vbo& vbo, Renderer::RenderContext& context, float angle) {
assert(hit != NULL);
Vec3f xAxis, yAxis, zAxis;
axes(context.camera().position(), xAxis, yAxis, zAxis);
Renderer::ActivateShader shader(context.shaderManager(), Renderer::Shaders::HandleShader);
Mat4f rotation;
if (hit->hitArea() == Model::RotateHandleHit::HAXAxis) {
rotation = rotationMatrix(angle, Vec3f::PosX);
Renderer::ApplyModelMatrix applyRotation(context.transformation(), rotation);
shader.setUniformVariable("Color", Color(1.0f, 1.0f, 1.0f, 0.25f));
Renderer::RingFigure(Axis::AX, yAxis, zAxis, m_ringRadius, m_ringThickness, 8).render(vbo, context);
shader.setUniformVariable("Color", Color(1.0f, 1.0f, 1.0f, 1.0f));
Renderer::CircleFigure(Axis::AX, 0.0f, 2.0f * Math<float>::Pi, m_ringRadius + m_ringThickness, 32, false).render(vbo, context);
} else if (hit->hitArea() == Model::RotateHandleHit::HAYAxis) {
rotation = rotationMatrix(angle, Vec3f::PosY);
Renderer::ApplyModelMatrix applyRotation(context.transformation(), rotation);
shader.setUniformVariable("Color", Color(1.0f, 1.0f, 1.0f, 0.25f));
Renderer::RingFigure(Axis::AY, xAxis, zAxis, m_ringRadius, m_ringThickness, 8).render(vbo, context);
shader.setUniformVariable("Color", Color(1.0f, 1.0f, 1.0f, 1.0f));
Renderer::CircleFigure(Axis::AY, 0.0f, 2.0f * Math<float>::Pi, m_ringRadius + m_ringThickness, 32, false).render(vbo, context);
} else {
rotation = rotationMatrix(angle, Vec3f::PosZ);
Renderer::ApplyModelMatrix applyRotation(context.transformation(), rotation);
shader.setUniformVariable("Color", Color(1.0f, 1.0f, 1.0f, 0.25f));
Renderer::RingFigure(Axis::AZ, xAxis, yAxis, m_ringRadius, m_ringThickness, 8).render(vbo, context);
shader.setUniformVariable("Color", Color(1.0f, 1.0f, 1.0f, 1.0f));
Renderer::CircleFigure(Axis::AZ, 0.0f, 2.0f * Math<float>::Pi, m_ringRadius + m_ringThickness, 32, false).render(vbo, context);
}
}
void NeutrinoEllipseCalculator::labSystemTransform()
{
//rotate Htilde to H
TMatrixD R(3,3);
R.Zero();
TMatrixD Rz=rotationMatrix(2,-lepton_.Phi());
TMatrixD Ry=rotationMatrix(1,0.5*M_PI-lepton_.Theta());
double bJetP[3]={bJet_.Px(),bJet_.Py(), bJet_.Pz()};
TMatrixD bJet_xyz(3,1,bJetP);
TMatrixD rM(Ry,TMatrixD::kMult,TMatrixD(Rz,TMatrixD::kMult,bJet_xyz));
double* rA=rM.GetMatrixArray();
double phi=-TMath::ATan2(rA[2],rA[1]);
TMatrixD Rx=rotationMatrix(0,phi);
R=TMatrixD(Rz,TMatrixD::kTransposeMult,TMatrixD(Ry,TMatrixD::kTransposeMult,Rx.T()));
H_=TMatrixD(R,TMatrixD::kMult,Htilde_);
//calculate Hperp
double Hvalues[9]={H_[0][0],H_[0][1],H_[0][2],H_[1][0],H_[1][1],H_[1][2],0,0,1};
TArrayD Harray(9,Hvalues);
Hperp_.SetMatrixArray(Harray.GetArray());
//calculate Nperp
TMatrixD HperpInv(Hperp_);
HperpInv.Invert();
TMatrixD U(3,3);
U.Zero();
U[0][0]=1;
U[1][1]=1;
U[2][2]=-1;
Nperp_=TMatrixD(HperpInv,TMatrixD::kTransposeMult,TMatrixD(U,TMatrixD::kMult,HperpInv));
}
void Camera::rotateCam(float xAngle, float yAngle)
{
float* yRMat = rotationMatrix(0.0f, 1.0f, 0.0f, xAngle);
float* xRMat = rotationMatrix(1.0f, 0.0f, 0.0f, yAngle);
multiplyMatrix(viewMatrix, yRMat);
multiplyMatrix(viewMatrix, xRMat);
}
void CameraPathControls::draw(const mat4& projection, const mat4 views[],
const vec3& lightPositionWorld,
const int& selectedControlPoint,
float deltaT) const {
/// Common drawing.
predraw();
/// Update the model matrix.
static float angle = 1.0f;
angle += deltaT * 1.0f;
mat4 rotationMatrix = mat4::Identity();
rotationMatrix(0,0) = +std::cos(angle);
rotationMatrix(0,1) = -std::sin(angle);
rotationMatrix(1,0) = +std::sin(angle);
rotationMatrix(1,1) = +std::cos(angle);
/// Update the content of the uniforms.
glUniformMatrix4fv( _projectionID, 1, GL_FALSE, projection.data());
glUniformMatrix4fv( _viewID, 1, GL_FALSE, views[0].data());
glUniform3fv(_lightPositionWorldID, 1, lightPositionWorld.data());
glUniform1i( _selectedControlPointID, selectedControlPoint);
glUniformMatrix4fv(_rotationMatrixID, 1, GL_FALSE, rotationMatrix.data());
/// Render from camera point of view to 'normal' FBOs.
glBindFramebuffer(GL_FRAMEBUFFER, framebufferIDs["controllerView"]);
_vertices->draw();
}
glm::dmat3 SceneGraphNode::calculateWorldRotation() const {
// recursive up the hierarchy if there are parents available
if (_parent) {
return rotationMatrix() * _parent->calculateWorldRotation();
}
else {
return rotationMatrix();
}
}
// This method builds the rotation matrix according to 'refLat' and
// 'refLon' values.
void XYZ2NED::init()
{
// First, let's resize rotation matrix and assign the proper values
rotationMatrix.resize(3,3);
// The clasical rotation matrix is transposed here for convenience
rotationMatrix(0,0) = -std::sin(refLat)*std::cos(refLon);
rotationMatrix(1,0) = -std::sin(refLat)*std::sin(refLon);
rotationMatrix(2,0) = std::cos(refLat);
rotationMatrix(0,1) = -std::sin(refLon);
rotationMatrix(1,1) = std::cos(refLon);
rotationMatrix(2,1) = 0.0;
rotationMatrix(0,2) = -std::cos(refLat)*std::cos(refLon);
rotationMatrix(1,2) = -std::cos(refLat)*std::sin(refLon);
rotationMatrix(2,2) = -std::sin(refLat);
// Then, fill the sets with the proper types
inputSet.clear();
inputSet.insert(TypeID::dx);
inputSet.insert(TypeID::dy);
inputSet.insert(TypeID::dz);
outputSet.clear();
outputSet.insert(TypeID::dLat);
outputSet.insert(TypeID::dLon);
outputSet.insert(TypeID::dH);
} // End of method 'XYZ2NED::init()'
int main(int argc, char **argv) {
static char * tmp = NULL;
static int tmpi;
ros::init(tmpi, &tmp, "image_processing", ros::init_options::NoSigintHandler);
ros::NodeHandle nh;
ImageProcessing imageProcessing(nh);
int i = 0;
ros::Rate loop_rate(10);
loop_rate.sleep();
while (imageProcessing.sourceImage.size().height == 0){
ros::spinOnce();
loop_rate.sleep();
}
while (ros::ok()){
cv::Mat image = imageProcessing.sourceImage;
cv::Mat out;
cv::cvtColor(image, out, CV_RGB2GRAY);
bool a = cv::findChessboardCorners(out, cvSize(5,4), imageProcessing.imagePoints);
if (a == true){
imageProcessing.found = 1;
}
else{
imageProcessing.found = 0;
}
if(imageProcessing.found == 1){
++i;
cv::solvePnP(imageProcessing.objectPoints, imageProcessing.imagePoints,
imageProcessing.cameraMatrix, imageProcessing.distCoeffs,
imageProcessing.rvec, imageProcessing.tvec);//solvePnP
cv::Mat_<double> rotationMatrix;
cv::Rodrigues(imageProcessing.rvec, rotationMatrix);//change rvec to matrix
cv::Mat pattern_pose =
(cv::Mat_<double>(4, 4) << rotationMatrix(0, 0), rotationMatrix(0, 1), rotationMatrix(
0, 2), imageProcessing.tvec(0), rotationMatrix(1, 0), rotationMatrix(
1, 1), rotationMatrix(1, 2), imageProcessing.tvec(1), rotationMatrix(
2, 0), rotationMatrix(2, 1), rotationMatrix(2, 2), imageProcessing
.tvec(2), 0, 0, 0, 1);//final matrix (grid position in camera frame)
imageProcessing.msg.error = imageProcessing.tvec(0);
}
if(imageProcessing.found == 0){
imageProcessing.msg.error = 0;
}
imageProcessing.msg.found = imageProcessing.found;
imageProcessing.grid_info_pub.publish(imageProcessing.msg);//publish message
ros::spinOnce();
}
return 0;
}
int main(int argc, char **argv) {
std::ofstream pnp_time;
pnp_time.open ("pnp_time.txt");
std::ofstream recognition_time;
recognition_time.open ("recognition_time.txt");
static char * tmp = NULL;
static int tmpi;
ros::init(tmpi, &tmp, "image_processing", ros::init_options::NoSigintHandler);
ros::NodeHandle nh;
ImageProcessing imageProcessing(nh);
int i = 0;
while (/*ros::ok()*/ i < 101 ){
auto start_recognition= std::chrono::high_resolution_clock::now(); //start time count
imageProcessing.imagePoints = imageProcessing.Generate2DPoints(); //updating image points
auto finish_recognition = std::chrono::high_resolution_clock::now(); //stop time count
if(imageProcessing.found == 1){
recognition_time << std::chrono::duration_cast<std::chrono::nanoseconds>(finish_recognition-start_recognition).count()<<"\n"; //save time
}
auto start_pnp = std::chrono::high_resolution_clock::now(); //start time count
if(imageProcessing.found == 1){
++i;
cv::solvePnP(imageProcessing.objectPoints, imageProcessing.imagePoints,
imageProcessing.cameraMatrix, imageProcessing.distCoeffs,
imageProcessing.rvec, imageProcessing.tvec);//solvePnP
cv::Mat_<double> rotationMatrix;
cv::Rodrigues(imageProcessing.rvec, rotationMatrix);//change rvec to matrix
cv::Mat pattern_pose =
(cv::Mat_<double>(4, 4) << rotationMatrix(0, 0), rotationMatrix(0, 1), rotationMatrix(
0, 2), imageProcessing.tvec(0), rotationMatrix(1, 0), rotationMatrix(
1, 1), rotationMatrix(1, 2), imageProcessing.tvec(1), rotationMatrix(
2, 0), rotationMatrix(2, 1), rotationMatrix(2, 2), imageProcessing
.tvec(2), 0, 0, 0, 1);//final matrix (grid position in camera frame)
imageProcessing.msg.error = imageProcessing.tvec(0);
}
if(imageProcessing.found == 0){
imageProcessing.msg.error = 0;
}
imageProcessing.msg.found = imageProcessing.found;
imageProcessing.grid_info_pub.publish(imageProcessing.msg);//publish message
auto finish_pnp = std::chrono::high_resolution_clock::now(); //stop time count
if(imageProcessing.found == 1){
pnp_time << std::chrono::duration_cast<std::chrono::nanoseconds>(finish_pnp-start_pnp).count()<<"\n"; //save time
}
ros::spinOnce();
}
pnp_time.close();
recognition_time.close();
return 0;
}
QSize KDChart::TextLayoutItem::calcSizeHint(
QFont fnt, QPoint& topLeftPt, QPoint& topRightPt, QPoint& bottomRightPt, QPoint& bottomLeftPt ) const
{
const QSize siz( unrotatedSizeHint( fnt ));
//qDebug() << "-------- siz: "<<siz;
if( ! mAttributes.rotation() ){
topLeftPt = QPoint(0,0);
topRightPt = QPoint(siz.width(),0);
bottomRightPt = QPoint(siz.width(),siz.height());
bottomLeftPt = QPoint(0,siz.height());
return siz;
}
const QRect rect(QPoint(0, 0), siz + QSize(4,4));
const qreal angle = PI * mAttributes.rotation() / 180.0;
const qreal cosAngle = cos( angle );
const qreal sinAngle = sin( angle );
QMatrix rotationMatrix(cosAngle, sinAngle, -sinAngle, cosAngle, 0, 0);
QPolygon rotPts;
rotPts << rotationMatrix.map(rect.topLeft())
<< rotationMatrix.map(rect.topRight())
<< rotationMatrix.map(rect.bottomRight())
<< rotationMatrix.map(rect.bottomLeft());
QSize rotSiz( rotPts.boundingRect().size() );
//qDebug() << "-------- KDChart::TextLayoutItem::calcSizeHint() returns:"<<rotSiz<<rotPts;
topLeftPt = rotPts[0];
topRightPt = rotPts[1];
bottomRightPt = rotPts[2];
bottomLeftPt = rotPts[3];
return rotSiz;
}
QRectF rotatedRect( const QRectF& oldRect, qreal angleInt, const QPointF& center )
{
const QRect rect( oldRect.translated( center ).toRect() );
const qreal angle = PI * angleInt / 180.0;
const qreal cosAngle = cos( angle );
const qreal sinAngle = sin( angle );
QMatrix rotationMatrix(cosAngle, sinAngle, -sinAngle, cosAngle, 0, 0);
QPolygon rotPts;
rotPts << rotationMatrix.map(rect.topLeft()) //QPoint(0,0)
<< rotationMatrix.map(rect.topRight())
<< rotationMatrix.map(rect.bottomRight())
<< rotationMatrix.map(rect.bottomLeft());
//<< rotatedPoint(rect.topRight(), angleInt, center).toPoint()
//<< rotatedPoint(rect.bottomRight(), angleInt, center).toPoint()
//<< rotatedPoint(rect.bottomLeft(), angleInt, center).toPoint();
return rotPts.boundingRect();
/*
const QPointF topLeft( rotatedPoint( oldRect.topLeft(), angle, center ) );
const QPointF topRight( rotatedPoint( oldRect.topRight(), angle, center ) );
const QPointF bottomLeft( rotatedPoint( oldRect.bottomLeft(), angle, center ) );
const QPointF bottomRight( rotatedPoint( oldRect.bottomRight(), angle, center ) );
const qreal x = qMin( qMin( topLeft.x(), topRight.x() ), qMin( bottomLeft.x(), topLeft.x() ) );
const qreal y = qMin( qMin( topLeft.y(), topRight.y() ), qMin( bottomLeft.y(), topLeft.y() ) );
const qreal width = qMax( qMax( topLeft.x(), topRight.x() ), qMax( bottomLeft.x(), topLeft.x() ) ) - x;
const qreal height = qMax( qMax( topLeft.y(), topRight.y() ), qMax( bottomLeft.y(), topLeft.y() ) ) - y;
return QRectF( x, y, width, height );
*/
}
void Material::SetUVTransform(const Vector2& offset, float rotation, const Vector2& repeat)
{
Matrix3x4 transform(Matrix3x4::IDENTITY);
transform.m00_ = repeat.x_;
transform.m11_ = repeat.y_;
transform.m03_ = -0.5f * transform.m00_ + 0.5f;
transform.m13_ = -0.5f * transform.m11_ + 0.5f;
Matrix3x4 rotationMatrix(Matrix3x4::IDENTITY);
rotationMatrix.m00_ = Cos(rotation);
rotationMatrix.m01_ = Sin(rotation);
rotationMatrix.m10_ = -rotationMatrix.m01_;
rotationMatrix.m11_ = rotationMatrix.m00_;
rotationMatrix.m03_ = 0.5f - 0.5f * (rotationMatrix.m00_ + rotationMatrix.m01_);
rotationMatrix.m13_ = 0.5f - 0.5f * (rotationMatrix.m10_ + rotationMatrix.m11_);
transform = rotationMatrix * transform;
Matrix3x4 offsetMatrix = Matrix3x4::IDENTITY;
offsetMatrix.m03_ = offset.x_;
offsetMatrix.m13_ = offset.y_;
transform = offsetMatrix * transform;
SetShaderParameter("UOffset", Vector4(transform.m00_, transform.m01_, transform.m02_, transform.m03_));
SetShaderParameter("VOffset", Vector4(transform.m10_, transform.m11_, transform.m12_, transform.m13_));
}
void ScreenSpaceFramebuffer::render(){
glm::vec2 resolution = OsEng.windowWrapper().currentWindowResolution();
glm::vec4 size = _size.value();
float xratio = _originalViewportSize.x / (size.z-size.x);
float yratio = _originalViewportSize.y / (size.w-size.y);;
if(!_renderFunctions.empty()){
glViewport (-size.x*xratio, -size.y*yratio, _originalViewportSize.x*xratio, _originalViewportSize.y*yratio);
GLint defaultFBO = _framebuffer->getActiveObject();
_framebuffer->activate();
glClearColor (0.0f, 0.0f, 0.0f, 0.0f);
glClear (GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glEnable(GL_ALPHA_TEST);
for(auto renderFunction : _renderFunctions){
(*renderFunction)();
}
_framebuffer->deactivate();
glBindFramebuffer(GL_FRAMEBUFFER, defaultFBO);
glViewport (0, 0, resolution.x, resolution.y);
glm::mat4 rotation = rotationMatrix();
glm::mat4 translation = translationMatrix();
glm::mat4 scale = scaleMatrix();
scale = glm::scale(scale, glm::vec3((1.0/xratio), -(1.0/yratio), 1.0f));
glm::mat4 modelTransform = rotation*translation*scale;
draw(modelTransform);
}
}
Matrix Camera::getRotationMatrix()
{
Vector3D look = this->lookDir-this->position;
D3DXVec3Normalize(&look, &look);
//this->lookDir = look;
Vector3D right;
D3DXVec3Cross(&right, &look, &this->up);
D3DXVec3Normalize(&right, &right);
this->rightDir = right;
Vector3D newUp;
D3DXVec3Cross(&newUp, &right, &look);
D3DXVec3Normalize(&newUp, &newUp);
//this->up = newUp;
D3DXVec3Normalize(&this->up, &this->up);
Matrix rotationMatrix(right.x, right.y, right.z,0,
newUp.x, newUp.y, newUp.z,0,
look.x, look.y, look.z,0,
0,0,0,1);
/*Matrix rotationMatrix(right.x, this->up.x,look.x, 0,
right.y, this->up.y, look.y,0,
right.z, this->up.z, look.z,0,
0,0,0,1);*/
return rotationMatrix;
}
//! [2]
void BasicOperations::paintEvent(QPaintEvent *)
{
double pi = 3.14;
double a = pi/180 * 45.0;
double sina = sin(a);
double cosa = cos(a);
QMatrix translationMatrix(1, 0, 0, 1, 50.0, 50.0);
QMatrix rotationMatrix(cosa, sina, -sina, cosa, 0, 0);
QMatrix scalingMatrix(0.5, 0, 0, 1.0, 0, 0);
QMatrix matrix;
matrix = scalingMatrix * rotationMatrix * translationMatrix;
QPainter painter(this);
painter.setPen(QPen(Qt::blue, 1, Qt::DashLine));
painter.drawRect(0, 0, 100, 100);
painter.setMatrix(matrix);
painter.setFont(QFont("Helvetica", 24));
painter.setPen(QPen(Qt::black, 1));
painter.drawText(20, 10, "QMatrix");
}
void renderScene(void) {
frame++;
time=glutGet(GLUT_ELAPSED_TIME);
if (MoveLight && (time - timebase2 > 10))
{
lightAngle = PI/36;
myMulti(lightPosition, rotationMatrix(0.0, 1.0, 0.0, lightAngle));
timebase2 = time;
}
if (time - timebase > 1000) {
sprintf(s,"FPS:%4.2f",
frame*1000.0/(time-timebase));
timebase = time;
frame = 0;
}
glutSetWindowTitle(s);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
//placeCam(10,2,10,0,2,-5);
placeCam(viewPosition[0],viewPosition[1],viewPosition[2],0,0,-5);
multiplyMatrix(viewMatrix, rotationMatrix(0.0,1.0,0.0, angle));
multiplyMatrix(viewMatrix, rotationMatrix(1.0,0.0,0.0, angle2));
glUseProgram(p);
setUniforms();
glBindVertexArray(vert[0]);
glDrawArrays(GL_TRIANGLES, 0, sizeof(vertices1));
float T[16];
setScale(T,0.5,0.5,0.5);
multiplyMatrix(viewMatrix, T);
setTransMatrix(T,4,0,0);
multiplyMatrix(viewMatrix, T);
setUniforms();
glBindVertexArray(vert[1]);
glDrawArrays(GL_TRIANGLES, 0, sizeof(vertices1));
glutSwapBuffers();
}
void SceneNode::set_modelstate(Vector3D state, bool radians){
Matrix4x4 model_transf= rotationMatrix('y',state[2],radians);
//traslation
model_transf[0][3]=state[0];
model_transf[2][3]=state[1];
//assign to model
m_geomodel->set_transform(model_transf);
}
static void fpsCameraViewMatrix(GLFWwindow *window, float *view)
{
// initial camera config
static float position[] = { 0.0f, 0.3f, 1.5f };
static float rotation[] = { 0.0f, 0.0f };
// mouse look
static double lastMouse[] = { 0.0, 0.0 };
double mouse[2];
glfwGetCursorPos(window, &mouse[0], &mouse[1]);
if (glfwGetMouseButton(window, GLFW_MOUSE_BUTTON_LEFT) == GLFW_PRESS)
{
rotation[0] += (float)(mouse[1] - lastMouse[1]) * -0.2f;
rotation[1] += (float)(mouse[0] - lastMouse[0]) * -0.2f;
}
lastMouse[0] = mouse[0];
lastMouse[1] = mouse[1];
float rotationY[16], rotationX[16], rotationYX[16];
rotationMatrix(rotationX, rotation[0], 1.0f, 0.0f, 0.0f);
rotationMatrix(rotationY, rotation[1], 0.0f, 1.0f, 0.0f);
multiplyMatrices(rotationYX, rotationY, rotationX);
// keyboard movement (WSADEQ)
float speed = (glfwGetKey(window, GLFW_KEY_LEFT_SHIFT) == GLFW_PRESS) ? 0.1f : 0.01f;
float movement[3] = {0};
if (glfwGetKey(window, GLFW_KEY_W) == GLFW_PRESS) movement[2] -= speed;
if (glfwGetKey(window, GLFW_KEY_S) == GLFW_PRESS) movement[2] += speed;
if (glfwGetKey(window, GLFW_KEY_A) == GLFW_PRESS) movement[0] -= speed;
if (glfwGetKey(window, GLFW_KEY_D) == GLFW_PRESS) movement[0] += speed;
if (glfwGetKey(window, GLFW_KEY_E) == GLFW_PRESS) movement[1] -= speed;
if (glfwGetKey(window, GLFW_KEY_Q) == GLFW_PRESS) movement[1] += speed;
float worldMovement[3];
transformPosition(worldMovement, rotationYX, movement);
position[0] += worldMovement[0];
position[1] += worldMovement[1];
position[2] += worldMovement[2];
// construct view matrix
float inverseRotation[16], inverseTranslation[16];
transposeMatrix(inverseRotation, rotationYX);
translationMatrix(inverseTranslation, -position[0], -position[1], -position[2]);
multiplyMatrices(view, inverseRotation, inverseTranslation); // = inverse(translation(position) * rotationYX);
}
///////////////////////////////////////////////////////////////////////////////
// Gets a position, rotation and scale and returns a modelview matrix
static const mat4 getModelView(const vec3& position = vec3(0.0f, 0.0f, 0.0f), const vec3& rotation = vec3(0.0f, 0.0f, 0.0f), const vec3& scale = vec3(1.0f, 1.0f, 1.0f)) {
mat4 rotationMatrix(1.f);
rotationMatrix = rotate(rotationMatrix, rotation.x, vec3(1.0f, 0.0f, 0.0f));
rotationMatrix = rotate(rotationMatrix, rotation.y, vec3(0.0f, 1.0f, 0.0f));
rotationMatrix = rotate(rotationMatrix, rotation.z, vec3(0.0f, 0.0f, 1.0f));
return translate(position) * rotationMatrix * glm::scale(scale);
}
void JointNode::set_joint_y(double min, double init, double max)
{
m_joint_y.min = min;
m_joint_y.init = init;
m_joint_y.max = max;
Matrix4x4 r = rotationMatrix('y', init);
m_trans = m_trans*r;
m_invtrans = r.invert()*m_invtrans;
}
MStatus AimNode::compute(const MPlug& plug, MDataBlock& dataBlock)
{
MStatus status;
if ((plug == aOutputRotate) || (plug == aOutputRotateX) ||
(plug == aOutputRotateY) || (plug == aOutputRotateZ))
{
// the matrix to aim at
MMatrix driverMatrix = dataBlock.inputValue(aDriverMatrix).asMatrix();
// the matrix to use as the upVector
MMatrix upVectorMatrix = dataBlock.inputValue(aUpVectorMatrix).asMatrix();
MVector inputTranslate = dataBlock.inputValue(aInputTranslate).asVector();
MMatrix parentInverse = dataBlock.inputValue(aParentInverseMatrix).asMatrix();
driverMatrix *= parentInverse;
upVectorMatrix *= parentInverse;
//extract the translation from the matrices
MVector driverMatrixPos(driverMatrix[3][0],
driverMatrix[3][1],
driverMatrix[3][2]);
MVector upVectorMatrixPos(upVectorMatrix[3][0],
upVectorMatrix[3][1],
upVectorMatrix[3][2]);
//get the vectors
MVector aimVector = driverMatrixPos - inputTranslate;
MVector upVector = upVectorMatrixPos - inputTranslate;
//upVector *= parentInverse;
aimVector.normalize();
upVector.normalize();
//perpendicter vector
MVector cross = aimVector ^ upVector;
upVector = cross ^ aimVector;
//build rotationMatrix
double tmpMatrix[4][4]{{aimVector.x, aimVector.y, aimVector.z, 0},
{upVector.x, upVector.y, upVector.z, 0},
{cross.x, cross.y, cross.z, 0},
{inputTranslate[0], inputTranslate[1], inputTranslate[2], 1}};
//eulerRotations
MMatrix rotationMatrix(tmpMatrix);
MTransformationMatrix matrixfn(rotationMatrix);
MEulerRotation eular = matrixfn.eulerRotation();
dataBlock.outputValue(aOutputRotate).set(eular.x, eular.y, eular.z);
dataBlock.outputValue(aOutputRotate).setClean();
}
return MS::kSuccess;
}
void JointNode::set_joint_x(double min, double init, double max)
{
m_joint_x.min = min;
m_joint_x.init = init;
m_joint_x.max = max;
m_joint_x.current=0.0;
Matrix4x4 r = rotationMatrix('x',init);
m_trans = m_trans*r;
m_invtrans = r.invert() * m_invtrans;
}
void TransformationMatrix::recompose(const DecomposedType& decomp)
{
makeIdentity();
// first apply perspective
m_matrix[0][3] = (float) decomp.perspectiveX;
m_matrix[1][3] = (float) decomp.perspectiveY;
m_matrix[2][3] = (float) decomp.perspectiveZ;
m_matrix[3][3] = (float) decomp.perspectiveW;
// now translate
translate3d((float) decomp.translateX, (float) decomp.translateY, (float) decomp.translateZ);
// apply rotation
double xx = decomp.quaternionX * decomp.quaternionX;
double xy = decomp.quaternionX * decomp.quaternionY;
double xz = decomp.quaternionX * decomp.quaternionZ;
double xw = decomp.quaternionX * decomp.quaternionW;
double yy = decomp.quaternionY * decomp.quaternionY;
double yz = decomp.quaternionY * decomp.quaternionZ;
double yw = decomp.quaternionY * decomp.quaternionW;
double zz = decomp.quaternionZ * decomp.quaternionZ;
double zw = decomp.quaternionZ * decomp.quaternionW;
// Construct a composite rotation matrix from the quaternion values
TransformationMatrix rotationMatrix(1 - 2 * (yy + zz), 2 * (xy - zw), 2 * (xz + yw), 0,
2 * (xy + zw), 1 - 2 * (xx + zz), 2 * (yz - xw), 0,
2 * (xz - yw), 2 * (yz + xw), 1 - 2 * (xx + yy), 0,
0, 0, 0, 1);
multLeft(rotationMatrix);
// now apply skew
if (decomp.skewYZ) {
TransformationMatrix tmp;
tmp.setM32((float) decomp.skewYZ);
multLeft(tmp);
}
if (decomp.skewXZ) {
TransformationMatrix tmp;
tmp.setM31((float) decomp.skewXZ);
multLeft(tmp);
}
if (decomp.skewXY) {
TransformationMatrix tmp;
tmp.setM21((float) decomp.skewXY);
multLeft(tmp);
}
// finally, apply scale
scale3d((float) decomp.scaleX, (float) decomp.scaleY, (float) decomp.scaleZ);
}
void Pattern::showPattern()
{
cout << "Pattern ID: " << id << endl;
cout << "Pattern Size: " << size << endl;
cout << "Pattern Confedince Value: " << confidence << endl;
cout << "Pattern Orientation: " << orientation << endl;
rotationMatrix(rotVec, rotMat);
cout << "Exterior Matrix (from pattern to camera): " << endl;
for (int i = 0; i<3; i++){
cout << rotMat.at<float>(i,0) << "\t" << rotMat.at<float>(i,1) << "\t" << rotMat.at<float>(i,2) << " |\t"<< transVec.at<float>(i,0) << endl;
}
}
QQuaternion GLC_Matrix4x4::quaternion() const
{
QQuaternion subject;
GLC_Matrix4x4 rotMat= rotationMatrix();
if ((this->type() != GLC_Matrix4x4::Identity) && (rotMat != GLC_Matrix4x4()))
{
const double matrixTrace= rotMat.trace();
double s, w, x, y, z;
if (matrixTrace > 0.0)
{
s= 0.5 / sqrt(matrixTrace);
w= 0.25 / s;
x= (rotMat.m_Matrix[9] - rotMat.m_Matrix[6]) * s;
y= (rotMat.m_Matrix[2] - rotMat.m_Matrix[8]) * s;
z= (rotMat.m_Matrix[4] - rotMat.m_Matrix[1]) * s;
}
else
{
if ((abs(rotMat.m_Matrix[0]) > abs(rotMat.m_Matrix[5])) && (abs(rotMat.m_Matrix[0]) > abs(rotMat.m_Matrix[15])))
{ // column 0 greater
s= sqrt(1.0 + rotMat.m_Matrix[0] - rotMat.m_Matrix[5] - rotMat.m_Matrix[10]) * 2.0;
w= (rotMat.m_Matrix[6] + rotMat.m_Matrix[9] ) / s;
x= 0.5 / s;
y= (rotMat.m_Matrix[1] + rotMat.m_Matrix[4] ) / s;
z= (rotMat.m_Matrix[2] + rotMat.m_Matrix[8] ) / s;
}
else if ((abs(rotMat.m_Matrix[5]) > abs(rotMat.m_Matrix[0])) && (abs(rotMat.m_Matrix[5]) > abs(rotMat.m_Matrix[15])))
{ // column 1 greater
s= sqrt(1.0 + rotMat.m_Matrix[5] - rotMat.m_Matrix[0] - rotMat.m_Matrix[10]) * 2.0;
w= (rotMat.m_Matrix[2] + rotMat.m_Matrix[8]) / s;
x= (rotMat.m_Matrix[1] + rotMat.m_Matrix[4]) / s;
y= 0.5 / s;
z= (rotMat.m_Matrix[6] + rotMat.m_Matrix[9]) / s;
}
else
{ // column 3 greater
s= sqrt(1.0 + rotMat.m_Matrix[10] - rotMat.m_Matrix[0] - rotMat.m_Matrix[5]) * 2.0;
w = (rotMat.m_Matrix[1] + rotMat.m_Matrix[4]) / s;
x = (rotMat.m_Matrix[2] + rotMat.m_Matrix[8]) / s;
y = (rotMat.m_Matrix[6] + rotMat.m_Matrix[9]) / s;
z = 0.5 / s;
}
}
subject= QQuaternion(w, x, y, z);
}
return subject;
}
bool PhotoCamera::initializeFromMeshLab(QDomElement &element)
{
QStringList attTemp;
//Get Translation
attTemp = element.attribute("TranslationVector").split(" ");
Eigen::Vector4f translation;
for(int i = 0; i < attTemp.count(); i++){
translation(i) = attTemp[i].toFloat();
}
translationMatrix.translation() = translation.head(3);
//translationMatrix.col(3) = translation;
//Get Center;
attTemp = element.attribute("CenterPx").split(" ");
principalPoint << attTemp.at(0).toFloat(), attTemp.at(1).toFloat();
//Get RotationMatrix;
attTemp = element.attribute("RotationMatrix").split(" ");
for(int i = 0; i < 4; i++){
for(int j = 0; j < 4; j++){
rotationMatrix(i, j) = attTemp[i*4 + j].toFloat();
}
}
//Get Viewport;
attTemp = element.attribute("ViewportPx").split(" ");
viewport << attTemp.at(0).toFloat(), attTemp.at(1).toFloat();
//Lens Distortion;
attTemp = element.attribute("LensDistortion").split(" ");
distortion << attTemp.at(0).toFloat(), attTemp.at(1).toFloat();
//std::cout << distortion.transpose() << std::endl;
//PixelSize;
attTemp = element.attribute("PixelSizeMm").split(" ");
pixelSize << attTemp.at(0).toFloat(), attTemp.at(1).toFloat();
oneOverDx = 1/pixelSize(0);
oneOverDy = 1/pixelSize(1);
//std::cout << pixelSize.transpose() << std::endl;
//Focal
focalLength = element.attribute("FocalMm").toFloat();
buildExtrinsic();
buildIntrinsic();
buildProjection();
// cameraCenter = center(intrinsicMatrix.topLeftCorner(3,4)*extrinsicMatrix.matrix());
cameraCenter = center(intrinsicMatrix.matrix().topLeftCorner(3,4)*extrinsicMatrix.matrix());
return true;
}
void mlTransform::SetMatrix(const mlMatrix3x4 & matrix)
{
mlMatrix3x3 rotationMatrix(matrix.I, matrix.J, matrix.K);
rotationMatrix.I.Normalise();
rotationMatrix.J.Normalise();
rotationMatrix.K.Normalise();
m_rotation = mlQuaternionFromRotationMatrix(rotationMatrix);
m_translation = matrix.T;
m_scale = matrix.I.Magnitude();
m_matrix_valid = false;
}
// This builds and returns a rotation matrix from the yaw and pitch rotations
mat4 Camera::GetRotationMatrix()
{
// Create an identity matrix
mat4 rotationMatrix(1.0f);
// Add the Pitch rotation along the x-axis
rotationMatrix = rotate(rotationMatrix, Pitch, vec3(1, 0, 0));
// Add the Yaw rotation along the y-axis
rotationMatrix = rotate(rotationMatrix, Yaw, vec3(0, 1, 0));
// Return the final rotation matrix that stores our camera rotations
return rotationMatrix;
}
void Gl1_PotentialBlock::go( const shared_ptr<Shape>& cm, const shared_ptr<State>& state ,bool wire2, const GLViewInfo&){
PotentialBlock* pp = static_cast<PotentialBlock*>(cm.get());
int shapeId = pp->id;
if(store == false) {
if(SF.size()>0) {
SF.clear();
initialized = false;
}
}
if(initialized == false ) {
FOREACH(const shared_ptr<Body>& b, *scene->bodies) {
if (!b) continue;
PotentialBlock* cmbody = dynamic_cast<PotentialBlock*>(b->shape.get());
if (!cmbody) continue;
Eigen::Matrix3d rotation = b->state->ori.toRotationMatrix(); //*pb->oriAabb.conjugate();
int count = 0;
for (int i=0; i<3; i++){
for (int j=0; j<3; j++){
//function->rotationMatrix[count] = directionCos(j,i);
rotationMatrix(i,j) = rotation(i,j); //input is actually direction cosine?
count++;
}
}
calcMinMax(*cmbody);
mc.init(sizeX,sizeY,sizeZ,min,max);
mc.resizeScalarField(scalarField,sizeX,sizeY,sizeZ);
SF.push_back(scalarF());
generateScalarField(*cmbody);
mc.computeTriangulation(scalarField,0.0);
SF[cmbody->id].triangles = mc.getTriangles();
SF[cmbody->id].normals = mc.getNormals();
SF[cmbody->id].nbTriangles = mc.getNbTriangles();
for(unsigned int i=0; i<scalarField.size(); i++) {
for(unsigned int j=0; j<scalarField[i].size(); j++) scalarField[i][j].clear();
scalarField[i].clear();
}
scalarField.clear();
}
initialized = true;
}
void Renderable::setMatrix() {
#ifdef OPENGL_ES1
glTranslatef(posx, posy, posz);
glRotatef(rot, rotx, roty, rotz);
glScalef(scalex, scaley, scalez);
#else
Mat16 tmp = translationMatrix(posx, posy, posz);
currentModelViewMatrix = multiplyMatrix(currentModelViewMatrix, tmp);
tmp = rotationMatrix(rot / 180 * M_PI, rotx, roty, rotz);
currentModelViewMatrix = multiplyMatrix(currentModelViewMatrix, tmp);
tmp = scaleMatrix(scalex, scaley, scalez);
currentModelViewMatrix = multiplyMatrix(currentModelViewMatrix, tmp);
glUniformMatrix4fv(shaderModelViewMatrix, 1, GL_FALSE, currentModelViewMatrix.m);
glUniformMatrix3fv(shaderNormalMatrix, 1, GL_FALSE, transposedInverseMatrix9(currentModelViewMatrix).m);
#endif
}
void EditPositionDialog::updateMatrix()
{
double alphaX= glc::toRadian(rx->value());
double alphaY= glc::toRadian(ry->value());
double alphaZ= glc::toRadian(rz->value());
GLC_Matrix4x4 rotationMatrix(GLC_Matrix4x4().fromEuler(alphaX, alphaY, alphaZ));
m_pOccurence->structInstance()->setMatrix(GLC_Matrix4x4(tx->value(), ty->value(), tz->value()) * rotationMatrix);
const int occurenceCount= m_pOccurence->structInstance()->numberOfOccurence();
QList<GLC_StructOccurence*> occurencesList= m_pOccurence->structInstance()->listOfStructOccurences();
for (int i= 0; i < occurenceCount; ++i)
{
occurencesList[i]->updateChildrenAbsoluteMatrix();
}
emit positionUpdated();
}