Cube::Cube(const Eigen::Affine3f& trafo, const Eigen::Vector4f& ambientColor, const Eigen::Vector4f& diffuseColor) {
vector<Eigen::Vector3f> vertices;
const float size = 0.5f;
vertices.push_back(Eigen::Vector3f( size, size, size));
vertices.push_back(Eigen::Vector3f( size, size, -size));
vertices.push_back(Eigen::Vector3f(-size, size, size));
vertices.push_back(Eigen::Vector3f(-size, size, -size));
vertices.push_back(Eigen::Vector3f(-size, -size, -size));
vertices.push_back(Eigen::Vector3f( size, -size, -size));
vertices.push_back(Eigen::Vector3f( size, -size, size));
vertices.push_back(Eigen::Vector3f(-size, -size, size));
vector<Eigen::Vector3f> normals(vertices.size());
transform(vertices.begin(), vertices.end(), normals.begin(), [] (const Eigen::Vector3f& v) {return v.normalized();});
for (auto& v : vertices) {
v = trafo * v;
}
vector<unsigned int> indices(14);
indices[ 0] = 0;
indices[ 1] = 1;
indices[ 2] = 2;
indices[ 3] = 3;
indices[ 4] = 4;
indices[ 5] = 1;
indices[ 6] = 5;
indices[ 7] = 6;
indices[ 8] = 4;
indices[ 9] = 7;
indices[10] = 2;
indices[11] = 6;
indices[12] = 0;
indices[13] = 1;
m_prog.addShaders(std::string(GLSL_PREFIX)+"mesh.vert", std::string(GLSL_PREFIX)+"mesh.frag");
m_prog.link();
m_prog.use();
m_prog.setUniformVec3("lightDir", Eigen::Vector3f(1.f, 0.5f, 2.f).normalized().data());
m_prog.setUniformVec3("clipNormal", Eigen::Vector3f(0.f, 0.f, 1.f).data());
m_prog.setUniformVar1f("clipDistance", 0.f);
m_prog.setUniformVec4("ambient", ambientColor.data());
m_prog.setUniformVec4("diffuse", diffuseColor.data());
m_geom.init();
m_geom.setVertices(vertices);
m_geom.setNormals(normals);
m_geom.setIndices(indices);
m_geom.enableVertices();
m_geom.enableNormals();
m_geom.enableIndices();
m_geom.upload();
m_geom.bindVertices(m_prog, "position");
m_geom.bindNormals(m_prog, "normal");
}
void AbsoluteEulerAngleDecoder::Decode(array_view<DirectX::Quaternion> rots, const VectorType & y)
{
int n = rots.size();
Eigen::Vector4f qs;
XMVECTOR q;
qs.setZero();
for (int i = 0; i < n; i++)
{
qs.segment<3>(0) = y.segment<3>(i * 3).cast<float>();
q = XMLoadFloat4A(qs.data());
q = XMQuaternionRotationRollPitchYawFromVector(q); // revert the log map
XMStoreA(rots[i], q);
}
}
void AbsoluteLnQuaternionDecoder::Decode(array_view<DirectX::Quaternion> rots, const VectorType & x)
{
int n = rots.size();
Eigen::Vector4f qs;
XMVECTOR q;
qs.setZero();
for (int i = 0; i < n; i++)
{
qs.segment<3>(0) = x.segment<3>(i * 3).cast<float>();
q = XMLoadFloat4A(qs.data());
q = XMQuaternionExp(q); // revert the log map
XMStoreA(rots[i], q);
}
}
void AbsoluteEulerAngleDecoder::Encode(array_view<const DirectX::Quaternion> rots, VectorType & x)
{
int n = rots.size();
Eigen::Vector4f qs;
XMVECTOR q;
qs.setZero();
x.resize(n * 3);
for (int i = 0; i < n; i++)
{
q = XMLoad(rots[i]);
q = XMQuaternionEulerAngleYawPitchRoll(q); // Decompsoe in to euler angle
XMStoreFloat4(qs.data(), q);
x.segment<3>(i * 3) = qs.head<3>();
}
}
void RelativeEulerAngleDecoder::Decode(array_view<DirectX::Quaternion> rots, const VectorType & x)
{
int n = rots.size();
Eigen::Vector4f qs;
XMVECTOR q, qb;
qs.setZero();
for (int i = 0; i < n; i++)
{
qs.segment<3>(0) = x.segment<3>(i * 3).cast<float>();
q = XMLoadFloat4A(qs.data());
q = XMQuaternionRotationRollPitchYawFromVector(q); // revert the log map
qb = XMLoadA(bases[i]);
q = XMQuaternionMultiply(qb, q);
XMStoreA(rots[i], q);
}
}
int main(int argc, char * argv[])
{
using namespace Eigen;
using namespace igl;
using namespace std;
// init mesh
string filename = "../shared/beast.obj";
if(argc < 2)
{
cerr<<"Usage:"<<endl<<" ./example input.obj"<<endl;
cout<<endl<<"Opening default mesh..."<<endl;
}else
{
// Read and prepare mesh
filename = argv[1];
}
// dirname, basename, extension and filename
string d,b,ext,f;
pathinfo(filename,d,b,ext,f);
// Convert extension to lower case
transform(ext.begin(), ext.end(), ext.begin(), ::tolower);
vector<vector<double > > vV,vN,vTC;
vector<vector<int > > vF,vFTC,vFN;
if(ext == "obj")
{
// Convert extension to lower case
if(!igl::readOBJ(filename,vV,vTC,vN,vF,vFTC,vFN))
{
return 1;
}
}else if(ext == "off")
{
// Convert extension to lower case
if(!igl::readOFF(filename,vV,vF,vN))
{
return 1;
}
}else if(ext == "wrl")
{
// Convert extension to lower case
if(!igl::readWRL(filename,vV,vF))
{
return 1;
}
//}else
//{
// // Convert extension to lower case
// MatrixXi T;
// if(!igl::readMESH(filename,V,T,F))
// {
// return 1;
// }
// //if(F.size() > T.size() || F.size() == 0)
// {
// boundary_faces(T,F);
// }
}
if(vV.size() > 0)
{
if(!list_to_matrix(vV,V))
{
return 1;
}
triangulate(vF,F);
}
// Compute normals, centroid, colors, bounding box diagonal
per_vertex_normals(V,F,N);
mid = 0.5*(V.colwise().maxCoeff() + V.colwise().minCoeff());
bbd = (V.colwise().maxCoeff() - V.colwise().minCoeff()).maxCoeff();
// Init embree
ei.init(V.cast<float>(),F.cast<int>());
// Init glut
glutInit(&argc,argv);
if( !TwInit(TW_OPENGL, NULL) )
{
// A fatal error occured
fprintf(stderr, "AntTweakBar initialization failed: %s\n", TwGetLastError());
return 1;
}
// Create a tweak bar
rebar.TwNewBar("TweakBar");
rebar.TwAddVarRW("scene_rot", TW_TYPE_QUAT4F, &scene_rot, "");
rebar.TwAddVarRW("lights_on", TW_TYPE_BOOLCPP, &lights_on, "key=l");
rebar.TwAddVarRW("color", TW_TYPE_COLOR4F, color.data(), "colormode=hls");
rebar.TwAddVarRW("ao_factor", TW_TYPE_DOUBLE, &ao_factor, "min=0 max=1 step=0.2 keyIncr=] keyDecr=[ ");
rebar.TwAddVarRW("ao_normalize", TW_TYPE_BOOLCPP, &ao_normalize, "key=n");
rebar.TwAddVarRW("ao_on", TW_TYPE_BOOLCPP, &ao_on, "key=a");
rebar.TwAddVarRW("light_intensity", TW_TYPE_DOUBLE, &light_intensity, "min=0 max=0.4 step=0.1 keyIncr=} keyDecr={ ");
rebar.load(REBAR_NAME);
glutInitDisplayString( "rgba depth double samples>=8 ");
glutInitWindowSize(glutGet(GLUT_SCREEN_WIDTH)/2.0,glutGet(GLUT_SCREEN_HEIGHT));
glutCreateWindow("ambient-occlusion");
glutDisplayFunc(display);
glutReshapeFunc(reshape);
glutKeyboardFunc(key);
glutMouseFunc(mouse);
glutMotionFunc(mouse_drag);
glutPassiveMotionFunc((GLUTmousemotionfun)TwEventMouseMotionGLUT);
glutMainLoop();
return 0;
}
int LoadPCD::compute()
{
//for each selected filename
for (int k = 0; k < m_filenames.size(); ++k)
{
Eigen::Vector4f origin;
Eigen::Quaternionf orientation;
QString filename = m_filenames[k];
boost::shared_ptr<PCLCloud> cloud_ptr_in = loadSensorMessage(filename, origin, orientation);
if (!cloud_ptr_in) //loading failed?
return 0;
PCLCloud::Ptr cloud_ptr;
if (!cloud_ptr_in->is_dense) //data may contain nans. Remove them
{
//now we need to remove nans
pcl::PassThrough<PCLCloud> passFilter;
passFilter.setInputCloud(cloud_ptr_in);
cloud_ptr = PCLCloud::Ptr(new PCLCloud);
passFilter.filter(*cloud_ptr);
}
else
{
cloud_ptr = cloud_ptr_in;
}
//now we construct a ccGBLSensor with these characteristics
ccGBLSensor * sensor = new ccGBLSensor;
// get orientation as rot matrix
Eigen::Matrix3f eigrot = orientation.toRotationMatrix();
// and copy it into a ccGLMatrix
ccGLMatrix ccRot;
for (int i = 0; i < 3; ++i)
{
for (int j = 0; j < 3; ++j)
{
ccRot.getColumn(j)[i] = eigrot(i,j);
}
}
ccRot.getColumn(3)[3] = 1.0;
// now in a format good for CloudComapre
//ccGLMatrix ccRot = ccGLMatrix::FromQuaternion(orientation.coeffs().data());
//ccRot = ccRot.transposed();
ccRot.setTranslation(origin.data());
sensor->setRigidTransformation(ccRot);
sensor->setDeltaPhi(static_cast<PointCoordinateType>(0.05));
sensor->setDeltaTheta(static_cast<PointCoordinateType>(0.05));
sensor->setVisible(true);
//uncertainty to some default
sensor->setUncertainty(static_cast<PointCoordinateType>(0.01));
ccPointCloud* out_cloud = sm2ccConverter(cloud_ptr).getCloud();
if (!out_cloud)
return -31;
sensor->setGraphicScale(out_cloud->getBB().getDiagNorm() / 10);
//do the projection on sensor
ccGenericPointCloud* cloud = ccHObjectCaster::ToGenericPointCloud(out_cloud);
int errorCode;
CCLib::SimpleCloud* projectedCloud = sensor->project(cloud,errorCode,true);
if (projectedCloud)
{
//DGM: we don't use it but we still have to delete it!
delete projectedCloud;
projectedCloud = 0;
}
QString cloud_name = QFileInfo(filename).baseName();
out_cloud->setName(cloud_name);
QFileInfo fi(filename);
QString containerName = QString("%1 (%2)").arg(fi.fileName()).arg(fi.absolutePath());
ccHObject* cloudContainer = new ccHObject(containerName);
out_cloud->addChild(sensor);
cloudContainer->addChild(out_cloud);
emit newEntity(cloudContainer);
}
return 1;
}
void Terrain::Draw(int type, const Camera& camera, const Light& light){
//Get new position of the cube and update the model view matrix
Eigen::Affine3f wMo;//object to world matrix
Eigen::Affine3f cMw;
Eigen::Affine3f proj;
glUseProgram(m_shader);
#ifdef __APPLE__
glBindVertexArrayAPPLE(m_vertexArrayObject);
#else
glBindVertexArray(m_vertexArrayObject);
#endif
glBindBuffer(GL_ARRAY_BUFFER, m_vertexBufferObject);//use as current buffer
GLint world2camera = glGetUniformLocation(m_shader, "cMw");
GLint projection = glGetUniformLocation(m_shader, "proj");
GLint kAmbient = glGetUniformLocation(m_shader,"kAmbient");
GLint kDiffuse = glGetUniformLocation(m_shader,"kDiffuse");
GLint kSpecular = glGetUniformLocation(m_shader,"kSpecular");
GLint shininess = glGetUniformLocation(m_shader,"shininess");
GLint camera_position = glGetUniformLocation(m_shader, "cameraPosition");
GLint light_position = glGetUniformLocation(m_shader, "lightPosition");
//generate the Angel::Angel::Angel::matrixes
proj = Util::Perspective( camera.m_fovy, camera.m_aspect, camera.m_znear, camera.m_zfar );
cMw = camera.m_cMw;//LookAt(camera.position,camera.lookat, camera.up );
Eigen::Vector4f v4color(0.55,0.25,0.08,1.0);
Eigen::Vector4f Ambient;
Ambient = 0.3*v4color;
Eigen::Vector4f Diffuse;
Diffuse = 0.5*v4color;
Eigen::Vector4f Specular(0.3,0.3,0.3,1.0);
glUniformMatrix4fv( world2camera, 1, GL_FALSE, cMw.data() );
glUniformMatrix4fv( projection, 1, GL_FALSE, proj.data() );
glUniform4fv(kAmbient, 1, Ambient.data());
glUniform4fv(kDiffuse, 1, Diffuse.data());
glUniform4fv(kSpecular, 1, Specular.data());
glUniform4fv(camera_position, 1, camera.m_position.data());
glUniform4fv(light_position, 1, light.m_position.data());
glUniform1f(shininess, 10);
switch (type) {
case DRAW_MESH:
glUniform1i(glGetUniformLocation(m_shader, "renderType"), 1);
glDrawArrays(GL_LINES, 0, m_NTrianglePoints);
break;
case DRAW_PHONG:
glUniform1i(glGetUniformLocation(m_shader, "renderType"), 2);
glDrawArrays(GL_TRIANGLES, 0, m_NTrianglePoints);
break;
}
//draw the obstacles
for(int i = 0; i < m_obstacles.size(); i++)
{
m_obstacles[i]->Draw(type,camera, light);
}
for(int i = 0; i < m_foods.size(); i++)
{
m_foods[i]->Draw(type,camera, light);
}
//draw the obstacles
for(int i = 0; i < m_surface_objects.size(); i++)
{
m_surface_objects[i]->Draw(type,camera, light);
}
}
void TW_CALL Viewer::snap_to_canonical_quaternion_cb(void *clientData)
{
Eigen::Vector4f snapq = static_cast<Viewer *>(clientData)->core.trackball_angle;
igl::snap_to_canonical_view_quat<float>(snapq.data(),1,static_cast<Viewer *>(clientData)->core.trackball_angle.data());
}
