float SHRot::dT(const int k, const int l1, const int m1, const int n1, const int l2, const int m2, const int n2) {
float result = 0.f;
for(int i = 0; i <= k; i++) {
result += binCoeff(k, i) * Ry(i, l1, m1, n1) * Ry(k-i, l2, m2, n2);
}
return result;
}
// Not implemented
TEST_F(SO3Test, RotationXYZ) {
EXPECT_CLOSE(Rx(0, 0), 1.0);
EXPECT_CLOSE(Rx(0, 1), 0.0);
EXPECT_CLOSE(Rx(0, 2), 0.0);
EXPECT_CLOSE(Rx(1, 0), 0.0);
EXPECT_CLOSE(Rx(1, 1), 0.5);
EXPECT_CLOSE(Rx(1, 2), -std::sqrt(3.0) / 2);
EXPECT_CLOSE(Rx(2, 0), 0.0);
EXPECT_CLOSE(Rx(2, 1), std::sqrt(3.0) / 2);
EXPECT_CLOSE(Rx(2, 2), 0.5);
EXPECT_CLOSE(Ry(0, 0), 0.5);
EXPECT_CLOSE(Ry(0, 1), 0.0);
EXPECT_CLOSE(Ry(0, 2), std::sqrt(3.0) / 2);
EXPECT_CLOSE(Ry(1, 0), 0.0);
EXPECT_CLOSE(Ry(1, 1), 1.0);
EXPECT_CLOSE(Ry(1, 2), 0.0);
EXPECT_CLOSE(Ry(2, 0), -std::sqrt(3.0) / 2);
EXPECT_CLOSE(Ry(2, 1), 0.0);
EXPECT_CLOSE(Ry(2, 2), 0.5);
EXPECT_CLOSE(Rz(0, 0), 0.5);
EXPECT_CLOSE(Rz(0, 1), -std::sqrt(3.0) / 2);
EXPECT_CLOSE(Rz(0, 2), 0.0);
EXPECT_CLOSE(Rz(1, 0), std::sqrt(3.0) / 2);
EXPECT_CLOSE(Rz(1, 1), 0.5);
EXPECT_CLOSE(Rz(1, 2), 0.0);
EXPECT_CLOSE(Rz(2, 0), 0.0);
EXPECT_CLOSE(Rz(2, 1), 0.0);
EXPECT_CLOSE(Rz(2, 2), 1.0);
}
Matrix & cartControlReachAvoidThread::roty(const double theta)
{
double t=CTRL_DEG2RAD*theta;
double c=cos(t);
double s=sin(t);
Ry(0,0)=Ry(2,2)=c;
Ry(0,2)=s;
Ry(2,0)=-s;
return Ry;
}
void SHRot::initDiagonals(int maxgrad) {
dySubDiag_.push_back(0.f);
dydyDiag_.push_back(0.f);
for(int l = 1; l <= maxgrad; l++) {
for(int help = -l; help <= l-1; help++) {
dySubDiag_.push_back(Ry(1, l, help+1, help));
}
for(int help2 = -l; help2 <= l; help2++) {
dydyDiag_.push_back(Ry(2, l, help2, help2));
}
}
}
Eigen::MatrixXd cameraMatrixKnownRT(const double rx, const double ry, const double rz, const double tx, const double ty, const double tz)
{
// Rotation matrix R = Rx * Ry * Rz(Euler's rotation theorem: any rotation can be broken into 3 angles)
// Rx rotation by rx about x:
Eigen::MatrixXd Rx(3,3), Ry(3,3), Rz(3,3);
Rx << cos(rx), -sin(rx), 0,
sin(rx), cos(rx), 0,
0, 0, 1;
// Ry rotation by ry about y:
Ry << cos(ry), 0, -sin(ry),
0, 1, 0,
sin(ry), 0, cos(ry);
// Rz rotation by rz about z:
Rz << cos(rz), sin(rz), 0,
-sin(rz), cos(rz), 0,
0, 0, 1;
Eigen::MatrixXd R = Rx * Ry * Rz;
Eigen::MatrixXd P(3,4);
P << R(0, 0), R(0, 1), R(0, 2), tx,
R(1, 0), R(1, 1), R(1, 2), ty,
R(2, 0), R(2, 1), R(2, 2), tz;
return P;
}
void display(void)
{
printError("pre display");
GLfloat t = (GLfloat)glutGet(GLUT_ELAPSED_TIME);
mat4 transMatrix = T(0.0f, 1.0f, -3.0f);
mat4 rotMatrix = Ry(0.1f);
mat4 total = Mult(rotMatrix, transMatrix);
//total = Mult(projectionMatrix, total);
glUniformMatrix4fv(glGetUniformLocation(program, "projectionMatrix"), 1, GL_TRUE, projectionMatrix);
//glUniformMatrix4fv(glGetUniformLocation(program, "transMatrix"), 1, GL_TRUE, transMatrix.m);
glUniformMatrix4fv(glGetUniformLocation(program, "totMatrix"), 1, GL_TRUE, total.m);
//glUniformMatrix4fv(glGetUniformLocation(program, "rotMatrix3"), 1, GL_TRUE, rotMatrix3);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// clear the screen
glBindVertexArray(bunnyVertexArrayObjID); // Select VAO
glDrawElements(GL_TRIANGLES, m->numIndices, GL_UNSIGNED_INT, 0L);
printError("display");
glutSwapBuffers();
}
void display(void)
{
printError("pre display");
// Update rotation matrix
t = (GLfloat)glutGet(GLUT_ELAPSED_TIME);
T(0,0,-3, trans);
Ry(0.001*t, rot);
Mult(trans, rot, total);
// clear the screen
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glBindVertexArray(bunnyVertexArrayObjID); // Select VAO
glDrawElements(GL_TRIANGLES, m->numIndices, GL_UNSIGNED_INT, 0L);
// Upload the Matrices
glUniformMatrix4fv(glGetUniformLocation(program, "projectionMatrix"), 1, GL_TRUE, projectionMatrix);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total);
// Upload texture unit
glUniform1i(glGetUniformLocation(program, "texUnit"), 0); // Texture unit 0
printError("display");
//glFlush();
glutSwapBuffers();
}
//------------------------------
void MolState::rotate(const double alpha_x, const double alpha_y, const double alpha_z)
{
// three rotation matrices (instead of making one matrix) arouns x, y, z axes
KMatrix R(CARTDIM,CARTDIM), Rx(CARTDIM,CARTDIM), Ry(CARTDIM,CARTDIM), Rz(CARTDIM,CARTDIM);
Rx.Set(0);
Rx.Elem2(0,0)=1;
Rx.Elem2(1,1)=cos(alpha_x);
Rx.Elem2(2,2)=cos(alpha_x);
Rx.Elem2(2,1)=-sin(alpha_x);
Rx.Elem2(1,2)=sin(alpha_x);
Ry.Set(0);
Ry.Elem2(1,1)=1;
Ry.Elem2(0,0)=cos(alpha_y);
Ry.Elem2(2,2)=cos(alpha_y);
Ry.Elem2(2,0)=sin(alpha_y);
Ry.Elem2(0,2)=-sin(alpha_y);
Rz.Set(0);
Rz.Elem2(2,2)=1;
Rz.Elem2(0,0)=cos(alpha_z);
Rz.Elem2(1,1)=cos(alpha_z);
Rz.Elem2(1,0)=-sin(alpha_z);
Rz.Elem2(0,1)=sin(alpha_z);
// overall rotation R=Rx*Ry*Rz
R.SetDiagonal(1);
R*=Rx;
R*=Ry;
R*=Rz;
// and now rotates using matix R:
transformCoordinates(R);
}
void dR_dthetax(Matrix3x3 &out, const Vector3 &theta)
{
Matrix3x3 mRy(Ry(theta.y()));
Matrix3x3 mRz(Rz(theta.z()));
Matrix3x3 mdRx_dtheta(dRx_dtheta(theta.x()));
out = mRz*mRy*mdRx_dtheta;
}
void dR_dthetaz(Matrix3x3 &out, const Vector3 &theta)
{
Matrix3x3 mRx(Rx(theta.x()));
Matrix3x3 mRy(Ry(theta.y()));
Matrix3x3 mdRz_dtheta(dRz_dtheta(theta.z()));
out = mdRz_dtheta*mRy*mRx;
}
//\fn void ExtrinsicParam::changePanTilt(double pan, double tilt);
///\brief This function computes the new rotation matrix and the new translation vector of the extrinsic parameters when the camera has changed its position.
///\param pan Value of the new camera panoramique.
///\param tilt Value of the new camera tilt.
void ExtrinsicParam::changePanTilt(double pan, double tilt)
{
// Compute the rotation matrices with the new values of pan and tilt
Eigen::Matrix3d Rx, Ry;
Rx.setZero();
Ry.setZero();
Rx(0,0) = 1;
Rx(1,1) = cos((-(tilt-this->tilt))*PI/180.);
Rx(1,2) = -sin((-(tilt-this->tilt))*PI/180.);
Rx(2,1) = sin((-(tilt-this->tilt))*PI/180.);
Rx(2,2) = cos((-(tilt-this->tilt))*PI/180.);
Ry(0,0) = cos((-(pan-this->pan))*PI/180.);
Ry(0,2) = sin((-(pan-this->pan))*PI/180.);
Ry(1,1) = 1;
Ry(2,0) = -sin((-(pan-this->pan))*PI/180.);
Ry(2,2) = cos((-(pan-this->pan))*PI/180.);
Eigen::Vector3d Tx, Ty;
Tx.setZero();
Ty.setZero();
Tx(0) = 2*3.3*sin(0.5*(this->pan-pan)*PI/180.)*cos(0.5*(this->pan-pan)*PI/180.);
Tx(2) = -2*3.3*sin(0.5*(this->pan-pan)*PI/180.)*cos((90.-0.5*(this->pan-pan))*PI/180.);
Ty(1) = 2*3.3*sin(0.5*(this->tilt-tilt)*PI/180.)*cos(0.5*(this->tilt-tilt)*PI/180.);
Ty(2) = -2*3.3*sin(0.5*(this->tilt-tilt)*PI/180.)*cos((90.-0.5*(this->tilt-tilt))*PI/180.);
// Compute the new values of the extrinsic parameters
Eigen::Matrix4d Rx1, Ry1, Rt;
Rt << initial_rotation, initial_translation,
0,0,0,1;
Rx1 << Rx, Tx,
0,0,0,1;
Ry1 << Ry, Ty,
0,0,0,1;
Rt.noalias() = Rx1*Ry1*Rt;
rotation(0,0) = Rt(0,0);rotation(0,1) = Rt(0,1);rotation(0,2) = Rt(0,2);
rotation(1,0) = Rt(1,0);rotation(1,1) = Rt(1,1);rotation(1,2) = Rt(1,2);
rotation(2,0) = Rt(2,0);rotation(2,1) = Rt(2,1);rotation(2,2) = Rt(2,2);
translation(0) = Rt(0,3);
translation(1) = Rt(1,3);
translation(2) = Rt(2,3);
}
void mouse(int x, int y)
{
vec3 camRight = Normalize(CrossProduct(camDir, camUp));
mat4 rot1;
mat4 rot2;
int dx = x - halfWindowWidth;
int dy = y - halfWindowHeight;
rot1 = ArbRotate(camRight, - dy * CAM_ROT_SPEED);
rot2 = Ry(-dx * CAM_ROT_SPEED);
rot2 = Mult(rot2, rot1);
camDir = Normalize(MultVec3(rot2, camDir));
glutWarpPointer(halfWindowWidth, halfWindowHeight);
}
void init_windmill(windmill_t* w, int n)
{
programs[WINDMILL_PROGRAM] = loadShaders("lab3-3.vert", "lab3-3.frag");
LoadTGATextureSimple("wood.tga", &billboards[WOOD_TEXTURE]);
LoadTGATextureSimple("brick.tga", &billboards[BRICK_TEXTURE]);
LoadTGATextureSimple("brick-crack.tga", &billboards[BRICK_CRACK_TEXTURE]);
w->bladeangle = 0;
w->windmill[WALLS] = LoadModelPlus("windmill/windmill-walls.obj", programs[WINDMILL_PROGRAM], "inPosition", "inNormal", "inTexCoord");
w->windmill[BALCONY] = LoadModelPlus("windmill/windmill-balcony.obj", programs[WINDMILL_PROGRAM], "inPosition", "inNormal", "inTexCoord");
w->windmill[ROOF] = LoadModelPlus("windmill/windmill-roof.obj", programs[WINDMILL_PROGRAM], "inPosition", "inNormal", "inTexCoord");
T(0.05,9.15,4.5,work[1]);
Ry(-M_PI_2,work[0]);
Mult(work[0], work[1], w->bladecenterMatrix);
{
int i = 0;
for(i = 0; i < 4; ++i) {
T(0,0,0,w->bladeMDLMatrix[i]);
Ry(100*M_PI/180,work[0]);
Mult(work[0], w->bladeMDLMatrix[i], work[1]);
Rz(M_PI_2*i,work[0]);
Mult(work[0], work[1], w->bladeMDLMatrix[i]);
w->blades[i] = LoadModelPlus("windmill/blade.obj", programs[WINDMILL_PROGRAM], "inPosition", "inNormal", "inTexCoord");
}
}
T((MILL_RADIUS + MILL_RADIUS/(n+1.0))*cos(2*M_PI/(double)NUMBER_OF_WINDMILLS*n),0,(MILL_RADIUS + MILL_RADIUS/(n+1.0))*sin(2*M_PI/(double)NUMBER_OF_WINDMILLS*n),work[0]);
double scale = 10*MILL_SCALE/(n+10.0);
S(scale, scale, scale, work[1]);
Mult(work[0], work[1], w->windmillMDLMatrix[WINDMILL_BASE]);
T(0,0,0,w->windmillMDLMatrix[WALLS]);
T(0,0,0,w->windmillMDLMatrix[BALCONY]);
T(0,0,0,w->windmillMDLMatrix[ROOF]);
printError("init windmill");
}
Point3D update_sphere(float t, GLfloat* total)
{
Point3D sphere_pos;
sphere_pos.x = 0;
sphere_pos.y = 0;
sphere_pos.z = 0;
T(60,0,0, trans);
Ry(0.01*t, roty);
Mult(roty, trans, total);
T(125,0,125, trans);
Mult(trans, total, total);
MatrixMultPoint3D(total, &sphere_pos, &sphere_pos);
return sphere_pos;
}
void dR_dtheta(Matrix3x3 &out, const Vector3 &theta, const Vector3 &theta_dot)
{
Matrix3x3 mRx(Rx(theta.x()));
Matrix3x3 mRy(Ry(theta.y()));
Matrix3x3 mRz(Rz(theta.z()));
Matrix3x3 mdRx_dtheta(dRx_dtheta(theta.x()));
Matrix3x3 mdRy_dtheta(dRx_dtheta(theta.y()));
Matrix3x3 mdRz_dtheta(dRx_dtheta(theta.z()));
out = mdRz_dtheta*mRy*mRx*theta_dot.z();
out += mRz*mdRy_dtheta*mRx*theta_dot.y();
out += mRz*mRy*mdRx_dtheta*theta_dot.x();
}
void mousedrag(int x, int y) {
static const double scale = 10.0;
static const double fiscale = 30.0;
Point3D diff;
diff.x = look_at.x - position.x;
diff.y = look_at.y - position.y;
diff.z = look_at.z - position.z;
Ry((float)(x-mouse_click[0])/fiscale, &work[0]);
MatrixMultPoint3D(&work[0], &diff, &diff);
VectorAdd(&position, &diff, &look_at);
look_at.y -= (float)(y-mouse_click[1])/scale;
//printf("Dragged to (%d,%d) yielding (%f,%f)\n", x, y, look_at.x, look_at.z);
mouse_click[0] = x;
mouse_click[1] = y;
}
//---------------------------------------------------------
bool CSG_Direct_Georeferencer::Set_Transformation(CSG_Parameters &Parameters, int nCols, int nRows)
{
//-----------------------------------------------------
m_O.Create(2);
m_O[0] = nCols / 2.0;
m_O[1] = nRows / 2.0;
m_f = Parameters("CFL" )->asDouble() / 1000; // [mm] -> [m]
m_s = Parameters("PXSIZE")->asDouble() / 1000000; // [micron] -> [m]
//-----------------------------------------------------
m_T.Create(3);
m_T[0] = Parameters("X")->asDouble();
m_T[1] = Parameters("Y")->asDouble();
m_T[2] = Parameters("Z")->asDouble();
//-----------------------------------------------------
double a;
CSG_Matrix Rx(3, 3), Ry(3, 3), Rz(3, 3);
a = Parameters("OMEGA")->asDouble() * M_DEG_TO_RAD;
Rx[0][0] = 1; Rx[0][1] = 0; Rx[0][2] = 0;
Rx[1][0] = 0; Rx[1][1] = cos(a); Rx[1][2] = -sin(a);
Rx[2][0] = 0; Rx[2][1] = sin(a); Rx[2][2] = cos(a);
a = Parameters("PHI" )->asDouble() * M_DEG_TO_RAD;
Ry[0][0] = cos(a); Ry[0][1] = 0; Ry[0][2] = sin(a);
Ry[1][0] = 0; Ry[1][1] = 1; Ry[1][2] = 0;
Ry[2][0] = -sin(a); Ry[2][1] = 0; Ry[2][2] = cos(a);
a = Parameters("KAPPA")->asDouble() * M_DEG_TO_RAD + Parameters("KAPPA_OFF")->asDouble() * M_DEG_TO_RAD;
Rz[0][0] = cos(a); Rz[0][1] = -sin(a); Rz[0][2] = 0;
Rz[1][0] = sin(a); Rz[1][1] = cos(a); Rz[1][2] = 0;
Rz[2][0] = 0; Rz[2][1] = 0; Rz[2][2] = 1;
switch( Parameters("ORIENTATION")->asInt() )
{
case 0: default: m_R = Rz * Rx * Ry; break; // BLUH
case 1: m_R = Rx * Ry * Rz; break; // PATB
}
m_Rinv = m_R.Get_Inverse();
return( true );
}
void OnTimer(int value)
{
static float e = 0;
mat4 rot, trans, scale, total;
scale = S(6, 6, 6);
rot = Ry(e);
trans = T(0, 0, 0);
total = Mult(trans, rot);
total = Mult(total, scale);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m); // // Upload matrix
e += 0.01;
glutPostRedisplay();
glutTimerFunc(20, &OnTimer, value);
}
void Body::rotate(char direction, float angle)
{
switch (direction) {
case 'x':
// Rotate around x
rot_mat = Rx(angle) * rot_mat;
break;
case 'y':
// Rotate around y
rot_mat = rot_mat * Ry(angle) * rot_mat;
break;
case 'z':
// Rotate around z
rot_mat = Rz(angle) * rot_mat;
break;
}
update();
}
void draw_obsticle(Maze* maze){
//gate
//OBSTACLES
int obstacle = maze->obstacle;
int obstacle_x_pos = maze->obstacle_x_pos;
mat4 obstacle_pos = T(obstacle_x_pos, 4, 8);
if ( obstacle == 1){ //BYT ALLT ANDREAS
//gate
glBindTexture(GL_TEXTURE_2D, gatetex);
glActiveTexture(GL_TEXTURE0);
//hö pelare
mat4 total = maze->get_total();
total = Mult(total, obstacle_pos);
total = Mult(total,S(2 , 6 , 2));
total = Mult(total, T(0,-0.25,2));
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
DrawModel(Gate,program,"inPosition","inNormal","inTexCoord");
//vänstra pelare
total = maze->get_total();
total = Mult(total, obstacle_pos);
total = Mult(total,S(2 , 6 , 2));
total = Mult(total, T(0,-0.25,-2));
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
DrawModel(Gate,program,"inPosition","inNormal","inTexCoord");
//tak
total = maze->get_total();
total = Mult(total, obstacle_pos);
total = Mult(total,S(2 , 2 , 6));
total = Mult(total, T(0,0.27,0));
total = Mult(total, Ry(M_PI_2));
total = Mult(total, Rz(M_PI_2));
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
DrawModel(Gate,program,"inPosition","inNormal","inTexCoord");
}
}
void display(void)
{
t = t+0.01;
trans = T(0, 0, 1);
rot = Ry(t);
total = Mult(rot, trans);
camera = lookAt(0.0f,1.0f,-2.0f,
0.0f,0.0f,0.0f,
0.0f,1.0f,0.0f);
printError("init shader");
printError("pre display");
// Matrixes
// clear the screen
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
rotationMatrix2[0] = cos(t);
rotationMatrix2[1] = -sin(t);
rotationMatrix2[4] = sin(t);
rotationMatrix2[5] = cos(t);
glUniformMatrix4fv(glGetUniformLocation(program, "rotationMatrix2"), 1, GL_TRUE, rotationMatrix2);
rotationMatrix[5] = cos(t/5.0);
rotationMatrix[6] = -sin(t/5.0);
rotationMatrix[9] = sin(t/5.0);
rotationMatrix[10] = cos(t/5.0);
glUniformMatrix4fv(glGetUniformLocation(program, "rotationMatrix"), 1, GL_TRUE, rotationMatrix);
glUniformMatrix4fv(glGetUniformLocation(program, "projMatrix"), 1, GL_TRUE, projectionMatrix);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
glUniformMatrix4fv(glGetUniformLocation(program, "Camera"), 1, GL_TRUE, camera.m);
glBindVertexArray(bunnyVertexArrayObjID); // Select VAO
glDrawElements(GL_TRIANGLES, m->numIndices, GL_UNSIGNED_INT, 0L);
printError("display");
glutSwapBuffers(); // Swap
}
// If the birds are flying side by side it looks weird if all of them looks att the same point
void Boid::setRotation()
{
// vec3 vn,vp;
//splitVector(direction,up,&vn,&vp);
vec3 v = forward;
vec3 v2 = Normalize(direction); //Normalize(vec3(direction.x, 0.0, direction.z));
//std::cout << "Direction for boid i: (" << direction.x << "," << direction.y << "," << direction.z << ")" << std::endl;
//float theta = acos(DotProduct(v2,v)/(Norm(v2)*Norm(v)));
float theta = 3.14 - atan2((v2.x-v.x),(v2.z-v.z));
//std::cout << "Theta = " << theta << std::endl;
rotationMatrix = Ry(theta);
forward = direction;
//rotationMatrix = ArbRotate(direction, theta); //Mult(yRot,xRot);
}
void display(void)
{
printError("pre display");
// clear the screen
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
mat4 m1, m2, m;
a += 0.1;
m1 = Rz(Pi/5);
m2 = Ry(a);
m = Mult(m2,m1);
glUniformMatrix4fv(glGetUniformLocation(program, "myMatrix"), 1, GL_TRUE, m.m);
glBindVertexArray(vertexArrayObjID); // Select VAO
glDrawElements(GL_TRIANGLES, 36, GL_UNSIGNED_BYTE, NULL); // draw object
printError("display");
glutSwapBuffers();
}
void display(void)
{
printError("pre display");
// clear the screen (using chosen color earlier)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glClearColor(1,1,1,0);
// Set rotation matrix
phi = ( phi < 2*PI ) ? phi+PI/50 : phi-2*PI+PI/50;
vec3 trans = {1,0,-3};
vec3 trans2 = {-1,0,-3};
vec3 lookAtPoint = {0,0,-3};
vec3 cameraLocation = {3.0f*cos(phi),0.0f,-3+3.0f*sin(phi)};
vec3 upVector = {0,1,0};
mat4 translation = T(trans.x,trans.y,trans.z);
mat4 rotations = Mult(Ry(phi),Mult(Rx(phi), Rz(phi)));
mat4 lookAtMatrix = lookAtv(cameraLocation,lookAtPoint,upVector);
transformMatrix = Mult(translation,rotations);
// Send translMatrix to Vertex
glUniformMatrix4fv(glGetUniformLocation(program, "transformMatrix"), 1, GL_TRUE, transformMatrix.m);
// Send lookAt-vector to vertex shader
glUniformMatrix4fv(glGetUniformLocation(program, "lookAtMatrix"), 1, GL_TRUE, lookAtMatrix.m);
DrawModel(bunny, program, "in_Position", "in_Normal", "inTexCoord");
// Model 2
transformMatrix = T(trans2.x, trans2.y, trans2.z);
glUniformMatrix4fv(glGetUniformLocation(program, "transformMatrix"), 1, GL_TRUE, transformMatrix.m);
DrawModel(bunny_model2, program, "in_Position", "in_Normal", "inTexCoord");
printError("display");
glutSwapBuffers(); // Swap buffer so that GPU can use the buffer we uploaded to it and we can write to another
}
void display(void) {
printError("pre display");
/* clear the screen*/
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
GLfloat t = (GLfloat)glutGet(GLUT_ELAPSED_TIME);
T(0, 0, -2, trans);
Ry(t/1000, rot);
Mult(trans, rot, total);
Rx(t/1000, rot);
Mult(total, rot, total);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total);
glBindVertexArray(bunnyVertexArrayObjID); // Select VAO
glDrawElements(GL_TRIANGLES, m->numIndices, GL_UNSIGNED_INT, 0L);
printError("display");
glutSwapBuffers();
}
void display(void)
{
printError("pre display");
a += 0.1;
mat4 rot, trans, total;
rot = Ry(a/2);
trans = T(0, 0, -5);
total = Mult(trans, rot);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total.m);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glBindVertexArray(bunnyVertexArrayObjID); // Select VAO
glDrawElements(GL_TRIANGLES, bunnyModel->numIndices, GL_UNSIGNED_INT, 0L);
printError("display");
glutSwapBuffers();
}
void display(void)
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
t += 0.02;
T(0,0,-10,trans);
Ry(t, rot);
Mult(trans, rot, total);
glUniformMatrix4fv(glGetUniformLocation(program, "mdlMatrix"), 1, GL_TRUE, total);
printError("pre display");
glBindVertexArray(bunnyVertexArrayObjID); // Select VAO
glDrawElements(GL_TRIANGLES, m->numIndices, GL_UNSIGNED_INT, 0L); // draw object
printError("display");
glutSwapBuffers();
}
void display(void)
{
printError("pre display");
// clear the screen (using chosen color earlier)
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Set rotation matrix
phi += PI/50;
mat4 translation = T(0,0,-3);
mat4 rotations = Mult(Ry(phi),Mult(Rx(phi), Rz(phi)));
transformMatrix = Mult(translation,rotations);
// Send translMatrix to Vertex
glUniformMatrix4fv(glGetUniformLocation(program, "transformMatrix"), 1, GL_TRUE, transformMatrix.m); // Variate shader
glBindVertexArray(bunnyVertexArrayObjID); // Select VAO
glDrawElements(GL_TRIANGLES, m->numIndices, GL_UNSIGNED_INT, 0L);
printError("display");
glutSwapBuffers(); // Swap buffer so that GPU can use the buffer we uploaded to it and we can write to another
}
void M2M(FmmvHandle *FMMV, Box *box)
{
int p = FMMV->pM;
int len0 = (p+1)*(p+1);
int len = (p+1)*(p+2);
int *P_RT = FMMV->P_MRT;
int *P_riri2rrii = FMMV->P_Mriri2rrii;
_FLOAT_ x1[(FMM_P_MAX+1)*(FMM_P_MAX+2)];
_FLOAT_ x2[(FMM_P_MAX+1)*(FMM_P_MAX+2)];
_FLOAT_ xx[(FMM_P_MAX+1)*(FMM_P_MAX+2)];
if (!isSource(box)) return;
if (isSource(box->child[SWD])&&box->child[SWD]->M) {
Rz_pi4(p, box->child[SWD]->M, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_pi_minus_theta, x1, x2);
perm(len0, P_RT, x2, x1);
Tz_M2M(FMMV, x1);
perm_inv(len0, P_RT, x1, xx);
if (isSource(box->child[NEU])&&box->child[NEU]->M) {
Rz_pi4(p, box->child[NEU]->M, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_minus_theta, x1, x2);
perm(len0, P_RT, x2, x1);
Tz_M2M(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry_pi(p, x2);
VEC_ADD(len, x2, xx, xx);
}
Ry(p, FMMV->Ry_minus_pi_minus_theta, xx, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_minus_pi4(p, x2, x1);
box->M = VEC_ADD2(FMMV, len, x1, box->M);
}
else if (isSource(box->child[NEU])&&box->child[NEU]->M) {
Rz_pi4(p, box->child[NEU]->M, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_minus_theta,x1, x2);
perm(len0, P_RT, x2, x1);
Tz_M2M(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry(p, FMMV->Ry_theta, x2, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_minus_pi4(p, x2, x1);
box->M = VEC_ADD2(FMMV, len, x1, box->M);
}
if (isSource(box->child[NWD])&&box->child[NWD]->M) {
Rz_minus_pi4(p, box->child[NWD]->M, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_pi_minus_theta, x1, x2);
perm(len0, P_RT, x2, x1);
Tz_M2M(FMMV, x1);
perm_inv(len0, P_RT, x1, xx);
if (isSource(box->child[SEU])&&box->child[SEU]->M) {
Rz_minus_pi4(p, box->child[SEU]->M, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_minus_theta, x1, x2);
perm(len0, P_RT, x2, x1);
Tz_M2M(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry_pi(p, x2);
VEC_ADD(len, x2, xx, xx);
}
Ry(p, FMMV->Ry_minus_pi_minus_theta, xx, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_pi4(p, x2, x1);
box->M = VEC_ADD2(FMMV, len, x1, box->M);
}
else if (isSource(box->child[SEU])&&box->child[SEU]->M) {
Rz_minus_pi4(p, box->child[SEU]->M, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_minus_theta, x1, x2);
perm(len0, P_RT, x2, x1);
Tz_M2M(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry(p, FMMV->Ry_theta, x2, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_pi4(p, x2, x1);
box->M = VEC_ADD2(FMMV, len, x1, box->M);
}
if (isSource(box->child[SED])&&box->child[SED]->M) {
Rz_3pi4(p, box->child[SED]->M, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_pi_minus_theta, x1, x2);
perm(len0, P_RT, x2, x1);
Tz_M2M(FMMV, x1);
perm_inv(len0, P_RT, x1, xx);
if (isSource(box->child[NWU])&&box->child[NWU]->M) {
Rz_3pi4(p, box->child[NWU]->M, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_minus_theta, x1, x2);
perm(len0, P_RT, x2, x1);
Tz_M2M(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry_pi(p, x2);
VEC_ADD(len, x2, xx, xx);
}
Ry(p, FMMV->Ry_minus_pi_minus_theta, xx, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_minus_3pi4(p, x2, x1);
box->M = VEC_ADD2(FMMV, len, x1, box->M);
}
else if (isSource(box->child[NWU])&&box->child[NWU]->M) {
Rz_3pi4(p, box->child[NWU]->M, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_minus_theta, x1, x2);
perm(len0, P_RT, x2, x1);
Tz_M2M(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry(p, FMMV->Ry_theta, x2, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_minus_3pi4(p, x2, x1);
box->M = VEC_ADD2(FMMV, len, x1, box->M);
}
if (isSource(box->child[NED])&&box->child[NED]->M) {
Rz_minus_3pi4(p, box->child[NED]->M, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_pi_minus_theta, x1, x2);
perm(len0, P_RT, x2, x1);
Tz_M2M(FMMV, x1);
perm_inv(len0, P_RT, x1, xx);
if (isSource(box->child[SWU])&&box->child[SWU]->M) {
Rz_minus_3pi4(p, box->child[SWU]->M, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_minus_theta, x1, x2);
perm(len0, P_RT, x2, x1);
Tz_M2M(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry_pi(p, x2);
VEC_ADD(len, x2, xx, xx);
}
Ry(p, FMMV->Ry_minus_pi_minus_theta, xx, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_3pi4(p, x2, x1);
box->M = VEC_ADD2(FMMV, len, x1, box->M);
}
else if (isSource(box->child[SWU])&&box->child[SWU]->M) {
Rz_minus_3pi4(p, box->child[SWU]->M, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_minus_theta, x1, x2);
perm(len0, P_RT, x2, x1);
Tz_M2M(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry(p, FMMV->Ry_theta, x2, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_3pi4(p, x2, x1);
box->M = VEC_ADD2(FMMV, len, x1, box->M);
}
}
void L2L(FmmvHandle *FMMV, Box *box)
{
int p = FMMV->pL;
int len0 = (p+1)*(p+1);
int len = (p+1)*(p+2);
int *P_RT = FMMV->P_LRT;
int *P_riri2rrii = FMMV->P_Lriri2rrii;
_FLOAT_ x1[(FMM_P_MAX+1)*(FMM_P_MAX+2)];
_FLOAT_ x2[(FMM_P_MAX+1)*(FMM_P_MAX+2)];
_FLOAT_ xx[(FMM_P_MAX+1)*(FMM_P_MAX+2)];
if (!isTarget(box)||!box->L) return;
if (isTarget(box->child[NEU])) {
Rz_pi4(p, box->L, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_pi_minus_theta, x1, xx);
perm(len0, P_RT, xx, x1);
Tz_L2L(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry(p, FMMV->Ry_minus_pi_minus_theta, x2, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_minus_pi4(p, x2, x1);
box->child[NEU]->L = VEC_ADD2(FMMV, len, x1, box->child[NEU]->L);
if (isTarget(box->child[SWD])) {
Ry_pi(p, xx);
perm(len0, P_RT, xx, x1);
Tz_L2L(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry(p, FMMV->Ry_theta, x2, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_minus_pi4(p, x2, x1);
box->child[SWD]->L = VEC_ADD2(FMMV, len, x1, box->child[SWD]->L);
}
}
else if (isTarget(box->child[SWD])) {
Rz_pi4(p, box->L, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_minus_theta, x1, x2);
perm(len0, P_RT, x2, x1);
Tz_L2L(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry(p, FMMV->Ry_theta, x2, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_minus_pi4(p, x2, x1);
box->child[SWD]->L = VEC_ADD2(FMMV, len, x1, box->child[SWD]->L);
}
if (isTarget(box->child[SEU])) {
Rz_minus_pi4(p, box->L, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_pi_minus_theta, x1, xx);
perm(len0, P_RT, xx, x1);
Tz_L2L(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry(p, FMMV->Ry_minus_pi_minus_theta, x2, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_pi4(p, x2, x1);
box->child[SEU]->L = VEC_ADD2(FMMV, len, x1, box->child[SEU]->L);
if (isTarget(box->child[NWD])) {
Ry_pi(p, xx);
perm(len0, P_RT, xx, x1);
Tz_L2L(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry(p, FMMV->Ry_theta, x2, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_pi4(p, x2, x1);
box->child[NWD]->L = VEC_ADD2(FMMV, len, x1, box->child[NWD]->L);
}
}
else if (isTarget(box->child[NWD])) {
Rz_minus_pi4(p, box->L, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_minus_theta, x1, x2);
perm(len0, P_RT, x2, x1);
Tz_L2L(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry(p, FMMV->Ry_theta, x2, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_pi4(p, x2, x1);
box->child[NWD]->L = VEC_ADD2(FMMV, len, x1, box->child[NWD]->L);
}
if (isTarget(box->child[NWU])) {
Rz_3pi4(p, box->L, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_pi_minus_theta, x1, xx);
perm(len0, P_RT, xx, x1);
Tz_L2L(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry(p, FMMV->Ry_minus_pi_minus_theta, x2, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_minus_3pi4(p, x2, x1);
box->child[NWU]->L = VEC_ADD2(FMMV, len, x1, box->child[NWU]->L);
if (isTarget(box->child[SED])) {
Ry_pi(p, xx);
perm(len0, P_RT, xx, x1);
Tz_L2L(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry(p, FMMV->Ry_theta, x2, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_minus_3pi4(p, x2, x1);
box->child[SED]->L = VEC_ADD2(FMMV, len, x1, box->child[SED]->L);
}
}
else if (isTarget(box->child[SED])) {
Rz_3pi4(p, box->L, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_minus_theta, x1, x2);
perm(len0, P_RT, x2, x1);
Tz_L2L(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry(p, FMMV->Ry_theta, x2, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_minus_3pi4(p, x2, x1);
box->child[SED]->L = VEC_ADD2(FMMV, len, x1, box->child[SED]->L);
}
if (isTarget(box->child[SWU])) {
Rz_minus_3pi4(p, box->L, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_pi_minus_theta, x1, xx);
perm(len0, P_RT, xx, x1);
Tz_L2L(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry(p, FMMV->Ry_minus_pi_minus_theta, x2, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_3pi4(p, x2, x1);
box->child[SWU]->L = VEC_ADD2(FMMV, len, x1, box->child[SWU]->L);
if (isTarget(box->child[NED])) {
Ry_pi(p, xx);
perm(len0, P_RT, xx, x1);
Tz_L2L(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry(p, FMMV->Ry_theta, x2, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_3pi4(p, x2, x1);
box->child[NED]->L = VEC_ADD2(FMMV, len, x1, box->child[NED]->L);
}
}
else if (isTarget(box->child[NED])) {
Rz_minus_3pi4(p, box->L, x2);
perm(len0, P_riri2rrii, x2, x1);
Ry(p, FMMV->Ry_minus_theta, x1, x2);
perm(len0, P_RT, x2, x1);
Tz_L2L(FMMV, x1);
perm_inv(len0, P_RT, x1, x2);
Ry(p, FMMV->Ry_theta, x2, x1);
perm_inv(len0, P_riri2rrii, x1, x2);
Rz_3pi4(p, x2, x1);
box->child[NED]->L = VEC_ADD2(FMMV, len, x1, box->child[NED]->L);
}
}