void Projector::_set_tilt (float theta, float phi)
{ //tilt image by theta,phi
//rotate model
mat_rot(2,0,phi,temp1); //move to edge of aperture
mat_rot(0,1,theta,temp2); //find "random" direction for aperture
mat_conj(temp2,temp1,tilt);
//correct for depth
float psi = 2.0f * phi * tanf(depth);
_bound_image(psi * cosf(theta), -psi * sinf(theta));
}
int test_getRotationMatrix2D()
{
cv::Mat matSrc = cv::imread("E:/GitCode/OpenCV_Test/test_images/lena.png", 1);
if (!matSrc.data) {
std::cout << "read image fail" << std::endl;
return -1;
}
double angle = -50.0;
double scale = 0.6;
fbc::Point2f center = fbc::Point2f(matSrc.cols / 2, matSrc.rows / 2);
fbc::Mat_<double, 1> mat_rot(2, 3);
fbc::getRotationMatrix2D(center, angle, scale, mat_rot);
// Compute a rotation matrix with respect to the center of the image
cv::Point center_ = cv::Point(matSrc.cols / 2, matSrc.rows / 2);
// Get the rotation matrix with the specifications above
cv::Mat mat_rot_ = cv::getRotationMatrix2D(center_, angle, scale);
assert(mat_rot.cols == mat_rot_.cols && mat_rot.rows == mat_rot_.rows);
assert(mat_rot.step == mat_rot_.step);
for (int y = 0; y < mat_rot.rows; y++) {
const fbc::uchar* p = mat_rot.ptr(y);
const uchar* p_ = mat_rot_.ptr(y);
for (int x = 0; x < mat_rot.step; x++) {
assert(p[x] == p_[x]);
}
}
return 0;
}
// computes the optimal rigid body transformation given a set of points
void PoseTracker::computeOptimalRigidTransformation(Eigen::MatrixXd startP, Eigen::MatrixXd finalP)
{
Eigen::Matrix4d transf;
if (startP.rows()!=finalP.rows())
{
ROS_ERROR("We need that the rows be the same at the beggining");
exit(1);
}
Eigen::RowVector3d centroid_startP = Eigen::RowVector3d::Zero();
Eigen::RowVector3d centroid_finalP = Eigen::RowVector3d::Zero(); //= mean(B);
Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
//calculate the mean
for (int i=0;i<startP.rows();i++)
{
centroid_startP = centroid_startP+startP.row(i);
centroid_finalP = centroid_finalP+finalP.row(i);
}
centroid_startP = centroid_startP/startP.rows();
centroid_finalP = centroid_finalP/startP.rows();
for (int i=0;i<startP.rows();i++)
H = H + (startP.row(i)-centroid_startP).transpose()*(finalP.row(i)-centroid_finalP);
Eigen::JacobiSVD<Eigen::MatrixXd> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
Eigen::MatrixXd U = svd.matrixU();
Eigen::MatrixXd V = svd.matrixV();
if (V.determinant()<0)
V.col(2)=-V.col(2)*(-1);
Eigen::MatrixXd R = V*U.transpose();
Eigen::Matrix4d C_A = Eigen::Matrix4d::Identity();
Eigen::Matrix4d C_B = Eigen::Matrix4d::Identity();
Eigen::Matrix4d R_new = Eigen::Matrix4d::Identity();
C_A.block<3,1>(0,3) = -centroid_startP.transpose();
R_new.block<3,3>(0,0) = R;
C_B.block<3,1>(0,3) = centroid_finalP.transpose();
transf = C_B * R_new * C_A;
Eigen::Quaterniond mat_rot(transf.block<3,3>(0,0));
Eigen::Vector3d trasl = transf.block<3,1>(0,3).transpose();
transfParameters_ << trasl(0), trasl(1), trasl(2), mat_rot.x(), mat_rot.y(), mat_rot.z(), mat_rot.w();
}
static void rotateAsteroid(gameComponentT* component, float dt) {
asteroidEntityDataT* asteroid = component->entity->data;
graphicsComponentDataT* gfx = getComponent(component->entity, "graphics")->data;
physicsComponentDataT* phys = getComponent(component->entity, "physics" )->data;
float o = bodyOrientation(phys->body);
//vec2 x = bodyPosition (phys->body);
mat4x4 m, r;
mat_identity (&m);
mat_rot_z (o, &r);
mat_mul (&r, &m, &m);
asteroid->angle1 += dt * asteroid->a1;
mat4x4 lol;
mat_rot(&asteroid->rot_axis_1, asteroid->angle1, &lol);
mat_mul(&lol, &m, &m);
gfx->transform = m;
}
static int LuaWrapper_AddLight(lua_State *L)
{
// lightpos, <lightrot>
int num_param = lua_gettop(L);
if (num_param != 2 && num_param != 1) {
printf("AddLight : Invalid input parameters.\n");
return 0;
}
float pos[3];
float xyz_rot[3];
if (3 != GetFloatArrayFromTable(L, 1, pos, 3))
goto ERROR_IN_AddLight;
if (num_param == 2) {
if (3 != GetFloatArrayFromTable(L, 2, xyz_rot, 3))
goto ERROR_IN_AddLight;
}
{
KMatrix4 mat;
nvmath::setTransMat(mat, KVec3(pos[0], pos[1], pos[2]));
nvmath::Quatf mat_rotX( KVec3(1,0,0), xyz_rot[0]/180.0f*nvmath::PI);
nvmath::Quatf mat_rotY( KVec3(1,0,0), xyz_rot[1]/180.0f*nvmath::PI);
nvmath::Quatf mat_rotZ( KVec3(1,0,0), xyz_rot[2]/180.0f*nvmath::PI);
nvmath::Quatf rot = mat_rotX * mat_rotY * mat_rotZ;
KMatrix4 mat_rot(rot);
KMatrix4 lightMat = mat_rot * mat;
float instensity[3] = {1.0f, 1.0f, 1.0f};
int idx = (int)KRT_AddLightSource("", (float*)&lightMat, instensity);
lua_pushnumber(L, idx);
return 0;
}
ERROR_IN_AddLight:
printf("AddLight : Invalid input parameters.\n");
return 0;
}
//#define WINDOWED
int main()
{
SDL_Init(SDL_INIT_VIDEO);
SDL_VideoInit(NULL);
SDL_GL_SetAttribute(SDL_GL_CONTEXT_MAJOR_VERSION, 3);
SDL_GL_SetAttribute(SDL_GL_CONTEXT_MINOR_VERSION, 2);
SDL_GL_SetAttribute(SDL_GL_MULTISAMPLEBUFFERS, 1);
SDL_GL_SetAttribute(SDL_GL_MULTISAMPLESAMPLES, 8);
#ifdef WINDOWED
SDL_Window *win = SDL_CreateWindow("hello world", 0, 0,640, 480, SDL_WINDOW_OPENGL);
#define ASPECT 640.0/480
#else
SDL_Window *win = SDL_CreateWindow("hello world", 0, 0,2560, 1440, SDL_WINDOW_OPENGL|SDL_WINDOW_FULLSCREEN);
#define ASPECT 2560.0/1440
#endif
SDL_ShowWindow(win);
SDL_GLContext context_dontneedtosavethis = SDL_GL_CreateContext(win);
SDL_GL_SetSwapInterval(0);
#ifdef WINDOWED
sm_screensize(640,480);
// glViewport(0,0,640,480);
#else
sm_screensize(2560,1440);
// glViewport(0,0,1366,768);
#endif
glEnable(GL_DEPTH_TEST);
glEnable(GL_BLEND);
glBlendFunc (GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glClearColor(0.3,0.5,1,0);
glClear(GL_COLOR_BUFFER_BIT);
if(sm_load("example"))return;
smm_open("examplemodel.tmd");
GLuint examplemodelvao = smm_upload();
int examplemodelnumtris =smm_numtris();
//========================feedback==============================
if(sm_load("examplefeedback"))return;
smm_open("exampleinstancing.tmd");//load the model to be instanced
GLuint instancedobjectsvao = smm_upload();
int instancedobjectsnumtris = smm_numtris();
glBindVertexArray(instancedobjectsvao);
GLuint instancedobjectspositionvbo;
glGenBuffers(1, &instancedobjectspositionvbo);//this is where we'll put the position offsets generated by the examplefeedback shader
glBindBuffer(GL_ARRAY_BUFFER, instancedobjectspositionvbo);
glBufferData(GL_ARRAY_BUFFER, sizeof(float)*3*500, NULL, GL_DYNAMIC_COPY);//allocate memory on gpu for 500 position vectors
glEnableVertexAttribArray(2);//enables the third attribute (offset)
glVertexAttribPointer(2, 3, GL_FLOAT, GL_FALSE, sizeof(float)*3, 0);//associates the attribute with the location and with the active vbo (instancedobjectspositionvbo)
glVertexAttribDivisor(2, 1);//set attribute 2 (offset) to change per instance instead of per vertex
glBindBuffer(GL_ARRAY_BUFFER, 0);
GLuint feedbackquery;//to count how many instanced objects got through the geometry shader (if used)
glGenQueries(1, &feedbackquery);
//========================/feedback==============================
if(sm_load("examplepp"))return;
if(sm_load("drawtext"))return;
unsigned char printtext[100] = {0};
GLuint textvao, textvbo;//manually setup the vao
glGenVertexArrays(1,&textvao);
glBindVertexArray(textvao);
glGenBuffers(1,&textvbo);
glBindBuffer(GL_ARRAY_BUFFER, textvbo);
glBufferData(GL_ARRAY_BUFFER, /*sizeof(vertexdata)*/strlen(printtext), printtext, GL_STREAM_DRAW);
glEnableVertexAttribArray(0);
glVertexAttribPointer(0, 1, GL_UNSIGNED_BYTE, GL_FALSE, 0, 0);//associates first attribute with vertexdata, also sets source to vbo
float modelviewproj[16];
mat_identity(modelviewproj);
mat_proj2(modelviewproj, 1.7453, ASPECT, 300, 0.1);
long tstart = SDL_GetTicks();
int i;
float rot =0;
float transx=0,transy=-5,transz=5;
float rotmat[16];
float wrot = 0;
float wroty = 0;
float speed = 0.001;
mat_identity(rotmat);
//SDL_SetRelativeMouseMode(SDL_TRUE);//this is not supported on all platforms, warping the mouse is more portable
for(i=0;i>=0;i++)
{
sm_use("example");
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
glBindVertexArray(examplemodelvao);
glDrawElements(GL_TRIANGLES, examplemodelnumtris*3, GL_UNSIGNED_SHORT, NULL);
sm_use("examplefeedback");
float wavetime = SDL_GetTicks() * 0.01;
glUniform1fv(sm_uniloc("wavetime"), 1,&wavetime);
glBindBufferBase(GL_TRANSFORM_FEEDBACK_BUFFER, 0, instancedobjectspositionvbo);//set output for transform feedback
glBeginQuery(GL_TRANSFORM_FEEDBACK_PRIMITIVES_WRITTEN, feedbackquery);//start counting
{
glBeginTransformFeedback(GL_POINTS);
{
glDrawArrays(GL_POINTS, 0, 500);//generate 500 position vectors
}glEndTransformFeedback();
}glEndQuery(GL_TRANSFORM_FEEDBACK_PRIMITIVES_WRITTEN);
GLuint feedbacknumgenerated;
glGetQueryObjectuiv(feedbackquery, GL_QUERY_RESULT, &feedbacknumgenerated);//get how many position vectors were generated
sm_use("example");
glBindVertexArray(instancedobjectsvao);
glDrawElementsInstanced(GL_TRIANGLES, instancedobjectsnumtris*3, GL_UNSIGNED_SHORT, NULL, feedbacknumgenerated);//draw the instances
sm_use("examplepp");
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
glDrawArrays(GL_POINTS, 0, 1);//we're using attributeless rendering, so we don't need a vao
sm_use("drawtext");
glDisable(GL_DEPTH_TEST);
glBindVertexArray(textvao);
glEnable(GL_DEPTH_TEST);
if(i%5==0)//calculate framerate every 5 frames
{
static long timesincelast;
memset(printtext, 0, 100);
long timethisframe = timesincelast-SDL_GetTicks();
if(timethisframe != 0)
SDL_itoa(1000*5/timethisframe, printtext, 10);
strcat(printtext, "FPS");
timesincelast = SDL_GetTicks();
glBufferData(GL_ARRAY_BUFFER, strlen(printtext), printtext, GL_STREAM_DRAW);//upload the text as an ordinary string
}
glDisable(GL_DEPTH_TEST);
glDrawArrays(GL_POINTS, 0,strlen(printtext));//draw the text to the screen
glEnable(GL_DEPTH_TEST);
SDL_GL_SwapWindow(win);
SDL_Event evt;
while(SDL_PollEvent(&evt))
{
if(evt.type == SDL_QUIT)
i= -i;
}
const char *keys = SDL_GetKeyboardState(NULL);
speed = 0.1;
if(keys[SDL_SCANCODE_LSHIFT])
speed = 2.8;
if(keys[SDL_SCANCODE_W])
{
transz -=speed*cos(wrot);
transx +=speed*sin(wrot);
}
if(keys[SDL_SCANCODE_A])
{
transz +=speed*cos(wrot-1.570796);
transx -=speed*sin(wrot-1.570796);
}
if(keys[SDL_SCANCODE_S])
{
transz +=speed*cos(wrot);
transx -=speed*sin(wrot);
}
if(keys[SDL_SCANCODE_D])
{
transz -=speed*cos(wrot-1.570796);
transx +=speed*sin(wrot-1.570796);
}
if(keys[SDL_SCANCODE_SPACE])
transy -=speed;
if(keys[SDL_SCANCODE_LCTRL])
transy +=speed;
int mousex, mousey;
//SDL_GetRelativeMouseState(&mousex, &mousey);
SDL_GetMouseState(&mousex, &mousey);
SDL_WarpMouseInWindow(win, 400,400);
mousex = mousex-400;
mousey = mousey-400;
wrot -= mousex/1000.0;
wroty-= mousey/1000.0;
wroty = wroty > 1.57?1.57:wroty<-1.57?-1.57:wroty;
float temp[16];
mat_identity(temp);
mat_mul(temp, modelviewproj);
mat_rot(temp, 1,0,0,wroty);
mat_rot(temp, 0,1,0,wrot);
mat_trans(temp, transx, transy, transz);
mat_mul(temp, rotmat);
mat_scale(temp, 10, 10, 10);
sm_use("example");
glUniformMatrix4fv(sm_uniloc("modelviewproj"), 1, GL_TRUE, temp);//upload the model-view-projection matrix
}
i = -i;
printf("last sdl error:%s\n", SDL_GetError());
printf("vendor:%s\n", glGetString(GL_RENDERER));
}
