COLLADASW::ColorOrTexture EffectsExporter::getcol(float r, float g, float b, float a)
{
COLLADASW::Color color(r, g, b, a);
COLLADASW::ColorOrTexture cot(color);
return cot;
}
//-----------------------------------------------------------------------
//read mesh from file
//-----------------------------------------------------------------------
void OMmodel::OpenMeshReadFile(const char * filename)
{
// request vertex normals, so the mesh reader can use normal information
// if available
mesh.request_vertex_normals();
// request face normals,
mesh.request_face_normals();
//request vertex color
mesh.request_vertex_colors();
// assure we have vertex normals
if (!mesh.has_vertex_normals())
{
std::cerr << "ERROR: Standard vertex property 'Normals' not available!\n";
}
OpenMesh::IO::Options opt;
// read mesh from file
if (!OpenMesh::IO::read_mesh(mesh, filename, opt))
{
std::cerr << "Error: Cannot read mesh from " << filename << std::endl;
}
// If the file did not provide vertex normals, then calculate them
if (!opt.check(OpenMesh::IO::Options::VertexNormal))
{
// let the mesh update the normals
mesh.update_normals();
}
// this vertex property stores the computed mean curvature
OpenMesh::VPropHandleT<double> HCurvature;
mesh.add_property(HCurvature);
//store valence
OpenMesh::VPropHandleT<int> valence;
mesh.add_property(valence);
//store gaussian curvature
OpenMesh::VPropHandleT<double> GCurvature;
mesh.add_property(GCurvature);
//caculate curvature and valence
vector<MyMesh::Point> oneRing;
float maxCur = 0;
float minCur = 0;
float maxGCur = 0;
float minGCur = 0;
int val = 0;
for (MyMesh::VertexIter v_it = mesh.vertices_begin(); v_it != mesh.vertices_end(); ++v_it)
{
oneRing.clear();
for (MyMesh::VertexVertexIter vv_it = mesh.vv_iter(*v_it); vv_it.is_valid(); ++vv_it)
{
++val;
oneRing.push_back(mesh.point(*vv_it));
}
OpenMesh::Vec3f H = OpenMesh::Vec3f(0.0f, 0.0f, 0.0f);
float A = 0;
float Gcurvature = 0;
float Hcurvature = 0;
float theta = 0;
float alpha = 0;
float beta = 0;
float arccosAlpha = 0;
float arccosBeta = 0;
float arccosTheta = 0;
float cotAlpha = 0;
float cotBeta = 0;
for (int i = 0; i < val; ++i)
{
OpenMesh::Vec3f Vvtovi, Vvtovi1, Vvi_1tovi, Vvi_1tov, Vvi1tovi, Vvi1tov, Vvitovi1;
MyMesh::Point PositionV = mesh.point(*v_it);
Vvtovi = oneRing[i] - PositionV;
OpenMesh::Vec3f nVvtovi = Vvtovi.normalized();
if (i == 0)
{
Vvi_1tovi = oneRing[0] - oneRing[val - 1];
Vvi_1tov = PositionV - oneRing[val - 1];
Vvi_1tovi.normalize();
Vvi_1tov.normalize();
}
else
{
Vvi_1tovi = oneRing[i] - oneRing[i - 1];
Vvi_1tov = PositionV - oneRing[i - 1];
Vvi_1tovi.normalize();
Vvi_1tov.normalize();
}
if (i == val - 1)
{
Vvtovi1 = oneRing[0] - PositionV;
Vvi1tovi = oneRing[i] - oneRing[0];
Vvi1tov = PositionV - oneRing[0];
Vvtovi1.normalize();
Vvi1tovi.normalize();
Vvi1tov.normalize();
}
else
{
Vvtovi1 = oneRing[i + 1] - PositionV;
Vvi1tovi = oneRing[i] - oneRing[i + 1];
Vvi1tov = PositionV - oneRing[i + 1];
Vvtovi1.normalize();
Vvi1tovi.normalize();
Vvi1tov.normalize();
}
alpha = dot(Vvi_1tovi, Vvi_1tov);
beta = dot(Vvi1tov, Vvi1tovi);
arccosAlpha = acos(alpha);
arccosBeta = acos(beta);
cotAlpha = cot(arccosAlpha);
cotBeta = cot(arccosBeta);
A += ((cotAlpha + cotBeta) * Vvtovi.sqrnorm());
H += ((cotAlpha + cotBeta) * Vvtovi);
//theta += Acos(Vvtovi, Vvtovi1, Vvi1tovi);
theta = dot(nVvtovi, Vvtovi1);
//debug2 << nVvtovi.sqrnorm()<<" "<<acos(theta) << endl;
arccosTheta += acos(theta);
}
//debug << arccosTheta << " " << H << " " << A << endl;;
A = A / 8.0f;
H = 2.0f * (H / A);
Hcurvature = 0.5f * H.norm();
Gcurvature = ((2.0f * M_PI) - arccosTheta)/ A;
maxCur = max(Hcurvature, maxCur);
minCur = min(Hcurvature, minCur);
if (Gcurvature < 1000000)
maxGCur = max(Gcurvature, maxGCur);
minGCur = min(Gcurvature, minGCur);
mesh.property(valence, *v_it) = val;
mesh.property(HCurvature, *v_it) = Hcurvature;
mesh.property(GCurvature, *v_it) = Gcurvature;
//debug << val << " " << Hcurvature << " " << Gcurvature << endl;
val = 0;
}
//std::cout << 2.0f * M_PI << endl;
//std::cout << maxCur << " " << minCur <<endl;
//std::cout << maxGCur << " " << minGCur << endl;
for (MyMesh::FaceIter f_it = mesh.faces_begin(); f_it != mesh.faces_end(); ++f_it)
{
for (MyMesh::FaceVertexIter fv_it = mesh.fv_iter(*f_it); fv_it.is_valid(); ++fv_it)
{
int value = mesh.property(valence, *fv_it);
if (value <= 4)
{
meshVertexColorBuffer.push_back(OpenMesh::Vec3f(0.0f, 0.0f, 1.0f));
}
if (value >= 5 && value <= 7)
{
meshVertexColorBuffer.push_back(OpenMesh::Vec3f(0.0f, 1.0f, 0.0f));
}
if (value >= 8)
{
meshVertexColorBuffer.push_back(OpenMesh::Vec3f(1.0f, 0.0f, 0.0f));
}
//cout << mesh.property(curvature, *fv_it) << endl;
meshCurColorBuffer.push_back(interporlationColor(maxCur, minCur, mesh.property(HCurvature, *fv_it), 1));
meshGCurColorBuffer.push_back(interporlationColor(maxGCur, minGCur, mesh.property(GCurvature, *fv_it), 10));
meshVertexBuffer.push_back(mesh.point(*fv_it));
meshVertexNormalBuffer.push_back(mesh.normal(*fv_it));
meshFaceNormalBuffer.push_back(mesh.normal(*f_it));
}
}
// don't need the normals anymore? Remove them!
mesh.release_vertex_normals();
// dispose the face normals, as we don't need them anymore
mesh.release_face_normals();
//release color
mesh.release_vertex_colors();
mesh.request_vertex_status();
meshVetexNum = mesh.n_vertices();
mesh.request_face_status();
meshFaceNum = mesh.n_faces();
mesh.request_halfedge_status();
meshHalfEdgeNum = mesh.n_halfedges();
// iterate over all halfedges
for (MyMesh::HalfedgeIter h_it = mesh.halfedges_begin(); h_it != mesh.halfedges_end(); ++h_it)
{
if (!mesh.face_handle(*h_it).is_valid())
{
++meshBoundryEdgeNum;
}
}
mesh.release_face_status();
mesh.release_vertex_status();
mesh.release_halfedge_status();
}
void Sensor::setVal(float roombaX, float roombaY, float roombaTheta){
std::cout << "roomba: " << roombaX << ", " << roombaY << std::endl;
float theta;
float x;
float y;
// 0:前 1:右 2:うしろ 3:左
float wallX[4];
float wallY[4];
//前方の壁の傾き
float wallTheta;
theta = roombaTheta;
// x = tan(theta)*(y - roombaY) + roombaX;
// y = tan(theta)*(x - roombaX) + roombaY;
// この2式をもとに、センサーとブロック境界の交点を求めていく。
//前後のセンサー
for(x = ceil(roombaX); x < BLOCK_SIZE * FIELD_WIDTH - HALF_BLOCK_SIZE; x += BLOCK_SIZE){
if((theta == M_PI_2) || (theta == M_PI_2 * 3)) break;
y = tan(theta)*(x - roombaX) + roombaY;
if((y < 0)||(y > BLOCK_SIZE * FIELD_HEIGHT)) break;
Block block = field->pointToBlock(x + HALF_BLOCK_SIZE, y);
if(field->map[block.y][block.x] == 1){
// std::cout << "(" << x << ", " << y << ")" << std::endl;
break;
// std::cout << "(" << block.x << ", " << block.y << ")" << std::endl;
}
}
if(((0 <= theta) && (theta < M_PI_2)) ||
((M_PI_2 * 3 <= theta)&&(theta < M_PI * 2))){
wallX[0] = x;
wallY[0] = y;
wallTheta = theta + M_PI_2;
if(wallTheta >= M_PI * 2) wallTheta -= M_PI * 2;
}else{
wallX[2] = x;
wallY[2] = y;
}
for(x = floor(roombaX); x > HALF_BLOCK_SIZE; x -= BLOCK_SIZE) {
if((theta == M_PI_2) || (theta == M_PI_2 * 3)) break;
y = tan(theta)*(x - roombaX) + roombaY;
if((y < 0)||(y > BLOCK_SIZE * FIELD_HEIGHT)) break;
Block block = field->pointToBlock(x - HALF_BLOCK_SIZE, y);
if(field->map[block.y][block.x] == 1){
// std::cout << "(" << x << ", " << y << ")" << std::endl;
break;
// std::cout << "(" << block.x << ", " << block.y << ")" << std::endl;
}
}
if(((0 <= theta) && (theta < M_PI_2)) ||
((M_PI_2 * 3 <= theta)&&(theta < M_PI * 2))){
wallX[2] = x;
wallY[2] = y;
}else{
wallX[0] = x;
wallY[0] = y;
wallTheta = theta - M_PI_2;
}
for(y = ceil(roombaY); y < BLOCK_SIZE * FIELD_HEIGHT - HALF_BLOCK_SIZE; y += BLOCK_SIZE){
if((theta == 0) || (theta == M_PI)) break;
x = cot(theta)*(y - roombaY) + roombaX;
if((x < 0)||(x > BLOCK_SIZE * FIELD_WIDTH)) break;
Block block = field->pointToBlock(x, y + HALF_BLOCK_SIZE);
if(field->map[block.y][block.x] == 1){
// std::cout << "(" << x << ", " << y << ")" << std::endl;
break;
// std::cout << "(" << block.x << ", " << block.y << ")" << std::endl;
}
}
if(((0 <= theta) && (theta < M_PI))){
if(y < wallY[0]){
wallX[0] = x;
wallY[0] = y;
wallTheta = theta;
}
}else{
if(y < wallY[2]){
wallX[2] = x;
wallY[2] = y;
}
}
for(y = floor(roombaY); y > HALF_BLOCK_SIZE; y -= BLOCK_SIZE){
if((theta == 0) || (theta == M_PI)) break;
x = cot(theta)*(y - roombaY) + roombaX;
if((x < 0)||(x > BLOCK_SIZE * FIELD_WIDTH)) break;
Block block = field->pointToBlock(x, y - HALF_BLOCK_SIZE);
if(field->map[block.y][block.x] == 1){
break;
// std::cout << "(" << x << ", " << y << ")" << std::endl;
// std::cout << "(" << block.x << ", " << block.y << ")" << std::endl;
}
}
if(((0 <= theta) && (theta < M_PI))){
if(y > wallY[2]){
wallX[2] = x;
wallY[2] = y;
}
}else{
if(y > wallY[0]){
wallX[0] = x;
wallY[0] = y;
wallTheta = theta - M_PI;
}
}
//左右のセンサー
theta += M_PI_2;
if(theta >= M_PI * 2) theta -= M_PI * 2;
for(x = ceil(roombaX); x < BLOCK_SIZE * FIELD_WIDTH - HALF_BLOCK_SIZE; x += BLOCK_SIZE){
if((theta == M_PI_2) || (theta == M_PI_2 * 3)) break;
y = tan(theta)*(x - roombaX) + roombaY;
if((y < 0)||(y > BLOCK_SIZE * FIELD_HEIGHT)) break;
Block block = field->pointToBlock(x + HALF_BLOCK_SIZE, y);
if(field->map[block.y][block.x] == 1){
// std::cout << "(" << x << ", " << y << ")" << std::endl;
break;
// std::cout << "(" << block.x << ", " << block.y << ")" << std::endl;
}
}
if(((0 <= theta) && (theta < M_PI_2)) ||
((M_PI_2 * 3 <= theta)&&(theta < M_PI * 2))){
wallX[1] = x;
wallY[1] = y;
}else{
wallX[3] = x;
wallY[3] = y;
}
for(x = floor(roombaX); x > HALF_BLOCK_SIZE; x -= BLOCK_SIZE) {
if((theta == M_PI_2) || (theta == M_PI_2 * 3)) break;
y = tan(theta)*(x - roombaX) + roombaY;
if((y < 0)||(y > BLOCK_SIZE * FIELD_HEIGHT)) break;
Block block = field->pointToBlock(x - HALF_BLOCK_SIZE, y);
if(field->map[block.y][block.x] == 1){
// std::cout << "(" << x << ", " << y << ")" << std::endl;
break;
// std::cout << "(" << block.x << ", " << block.y << ")" << std::endl;
}
}
if(((0 <= theta) && (theta < M_PI_2)) ||
((M_PI_2 * 3 <= theta)&&(theta < M_PI * 2))){
wallX[3] = x;
wallY[3] = y;
}else{
wallX[1] = x;
wallY[1] = y;
}
for(y = ceil(roombaY); y < BLOCK_SIZE * FIELD_HEIGHT - HALF_BLOCK_SIZE; y += BLOCK_SIZE){
if((theta == 0) || (theta == M_PI)) break;
x = cot(theta)*(y - roombaY) + roombaX;
if((x < 0)||(x > BLOCK_SIZE * FIELD_WIDTH)) break;
Block block = field->pointToBlock(x, y + HALF_BLOCK_SIZE);
if(field->map[block.y][block.x] == 1){
// std::cout << "(" << x << ", " << y << ")" << std::endl;
break;
// std::cout << "(" << block.x << ", " << block.y << ")" << std::endl;
}
}
if(((0 <= theta) && (theta < M_PI))){
if(y < wallY[1]){
wallX[1] = x;
wallY[1] = y;
}
}else{
if(y < wallY[3]){
wallX[3] = x;
wallY[3] = y;
}
}
for(y = floor(roombaY); y > HALF_BLOCK_SIZE; y -= BLOCK_SIZE){
if((theta == 0) || (theta == M_PI)) break;
x = cot(theta)*(y - roombaY) + roombaX;
if((x < 0)||(x > BLOCK_SIZE * FIELD_WIDTH)) break;
Block block = field->pointToBlock(x, y - HALF_BLOCK_SIZE);
if(field->map[block.y][block.x] == 1){
break;
// std::cout << "(" << x << ", " << y << ")" << std::endl;
// std::cout << "(" << block.x << ", " << block.y << ")" << std::endl;
}
}
if(((0 <= theta) && (theta < M_PI))){
if(y > wallY[3]){
wallX[3] = x;
wallY[3] = y;
}
}else{
if(y > wallY[1]){
wallX[1] = x;
wallY[1] = y;
}
}
std::cout << "front wall: " << wallX[0] << ", " << wallY[0] << std::endl;
std::cout << "back wall: " << wallX[2] << ", " << wallY[2] << std::endl;
std::cout << "right wall: " << wallX[1] << ", " << wallY[1] << std::endl;
std::cout << "left wall: " << wallX[3] << ", " << wallY[3] << std::endl;
std::cout << "wall theta: " << wallTheta * 180 / M_PI << std::endl;
}
