void
calculate_data_costs(mve::TriangleMesh::ConstPtr mesh, std::vector<TextureView> * texture_views,
Settings const & settings, ST * data_costs) {
mve::TriangleMesh::FaceList const & faces = mesh->get_faces();
mve::TriangleMesh::VertexList const & vertices = mesh->get_vertices();
mve::TriangleMesh::NormalList const & face_normals = mesh->get_face_normals();
std::size_t const num_faces = faces.size() / 3;
std::size_t const num_views = texture_views->size();
CollisionModel3D* model = newCollisionModel3D(true);
if (settings.geometric_visibility_test) {
/* Build up acceleration structure for the visibility test. */
ProgressCounter face_counter("\tBuilding collision model", num_faces);
model->setTriangleNumber(num_faces);
for (std::size_t i = 0; i < faces.size(); i += 3) {
face_counter.progress<SIMPLE>();
math::Vec3f v1 = vertices[faces[i]];
math::Vec3f v2 = vertices[faces[i + 1]];
math::Vec3f v3 = vertices[faces[i + 2]];
model->addTriangle(*v1, *v2, *v3);
face_counter.inc();
}
model->finalize();
}
std::vector<std::vector<ProjectedFaceInfo> > projected_face_infos(num_faces);
ProgressCounter view_counter("\tCalculating face qualities", num_views);
#pragma omp parallel
{
std::vector<std::pair<std::size_t, ProjectedFaceInfo> > projected_face_view_infos;
#pragma omp for schedule(dynamic)
for (std::uint16_t j = 0; j < texture_views->size(); ++j) {
view_counter.progress<SIMPLE>();
TextureView * texture_view = &texture_views->at(j);
texture_view->load_image();
texture_view->generate_validity_mask();
if (settings.data_term == GMI) {
texture_view->generate_gradient_magnitude();
texture_view->erode_validity_mask();
}
math::Vec3f const & view_pos = texture_view->get_pos();
math::Vec3f const & viewing_direction = texture_view->get_viewing_direction();
for (std::size_t i = 0; i < faces.size(); i += 3) {
std::size_t face_id = i / 3;
math::Vec3f const & v1 = vertices[faces[i]];
math::Vec3f const & v2 = vertices[faces[i + 1]];
math::Vec3f const & v3 = vertices[faces[i + 2]];
math::Vec3f const & face_normal = face_normals[face_id];
math::Vec3f const face_center = (v1 + v2 + v3) / 3.0f;
/* Check visibility and compute quality */
math::Vec3f view_to_face_vec = (face_center - view_pos).normalized();
math::Vec3f face_to_view_vec = (view_pos - face_center).normalized();
/* Backface culling */
float viewing_angle = face_to_view_vec.dot(face_normal);
if (viewing_angle < 0.0f || viewing_direction.dot(view_to_face_vec) < 0.0f)
continue;
if (std::acos(viewing_angle) > MATH_DEG2RAD(75.0f))
continue;
/* Projects into the valid part of the TextureView? */
if (!texture_view->inside(v1, v2, v3))
continue;
if (settings.geometric_visibility_test) {
/* Viewing rays do not collide? */
bool visible = true;
math::Vec3f const * samples[] = {&v1, &v2, &v3};
// TODO: random monte carlo samples...
for (std::size_t k = 0; k < sizeof(samples) / sizeof(samples[0]); ++k) {
math::Vec3f vertex = *samples[k];
math::Vec3f dir = view_pos - vertex;
float const dir_length = dir.norm();
dir.normalize();
if (model->rayCollision(*vertex, *dir, false, dir_length * 0.0001f, dir_length)) {
visible = false;
break;
}
}
if (!visible) continue;
}
ProjectedFaceInfo info = {j, 0.0f, math::Vec3f(0.0f, 0.0f, 0.0f)};
/* Calculate quality. */
texture_view->get_face_info(v1, v2, v3, &info, settings);
if (info.quality == 0.0) continue;
/* Change color space. */
mve::image::color_rgb_to_ycbcr(*(info.mean_color));
std::pair<std::size_t, ProjectedFaceInfo> pair(face_id, info);
projected_face_view_infos.push_back(pair);
}
texture_view->release_image();
texture_view->release_validity_mask();
if (settings.data_term == GMI) {
texture_view->release_gradient_magnitude();
}
view_counter.inc();
}
//std::sort(projected_face_view_infos.begin(), projected_face_view_infos.end());
#pragma omp critical
{
for (std::size_t i = projected_face_view_infos.size(); 0 < i; --i) {
std::size_t face_id = projected_face_view_infos[i - 1].first;
ProjectedFaceInfo const & info = projected_face_view_infos[i - 1].second;
projected_face_infos[face_id].push_back(info);
}
projected_face_view_infos.clear();
}
}
delete model;
model = NULL;
ProgressCounter face_counter("\tPostprocessing face infos", num_faces);
#pragma omp parallel for schedule(dynamic)
for (std::size_t i = 0; i < projected_face_infos.size(); ++i) {
face_counter.progress<SIMPLE>();
std::vector<ProjectedFaceInfo> & infos = projected_face_infos[i];
if (settings.outlier_removal != NONE) {
photometric_outlier_detection(&infos, settings);
infos.erase(std::remove_if(infos.begin(), infos.end(),
[](ProjectedFaceInfo const & info) -> bool {return info.quality == 0.0f;}),
infos.end());
}
std::sort(infos.begin(), infos.end());
face_counter.inc();
}
/* Determine the function for the normlization. */
float max_quality = 0.0f;
for (std::size_t i = 0; i < projected_face_infos.size(); ++i)
for (std::size_t j = 0; j < projected_face_infos[i].size(); ++j)
max_quality = std::max(max_quality, projected_face_infos[i][j].quality);
Histogram hist_qualities(0.0f, max_quality, 10000);
for (std::size_t i = 0; i < projected_face_infos.size(); ++i)
for (std::size_t j = 0; j < projected_face_infos[i].size(); ++j)
hist_qualities.add_value(projected_face_infos[i][j].quality);
float percentile = hist_qualities.get_approx_percentile(0.995f);
/* Calculate the costs. */
assert(num_faces < std::numeric_limits<std::uint32_t>::max());
assert(num_views < std::numeric_limits<std::uint16_t>::max());
assert(MRF_MAX_ENERGYTERM < std::numeric_limits<float>::max());
for (std::uint32_t i = 0; i < static_cast<std::uint32_t>(projected_face_infos.size()); ++i) {
for (std::size_t j = 0; j < projected_face_infos[i].size(); ++j) {
ProjectedFaceInfo const & info = projected_face_infos[i][j];
/* Clamp to percentile and normalize. */
float normalized_quality = std::min(1.0f, info.quality / percentile);
float data_cost = (1.0f - normalized_quality) * MRF_MAX_ENERGYTERM;
data_costs->set_value(i, info.view_id, data_cost);
}
/* Ensure that all memory is freeed. */
projected_face_infos[i].clear();
projected_face_infos[i].shrink_to_fit();
}
std::cout << "\tMaximum quality of a face within an image: " << max_quality << std::endl;
std::cout << "\tClamping qualities to " << percentile << " within normalization." << std::endl;
}
///////////////////
// Check Resolve a collision between 2 cars
void Car_CheckCarCollisions(CCar *pcCar1, CCar *pcCar2)
{
float m1[16], m2[16];
carsim_t *psCar1 = pcCar1->getCarSim();
carsim_t *psCar2 = pcCar2->getCarSim();
// First, do a quick check to make sure the cars are within a small
// distance of each other using a seperating axis check
if( fabs(psCar1->cPos.GetX() - psCar2->cPos.GetX()) > 50 )
return;
if( fabs(psCar1->cPos.GetY() - psCar2->cPos.GetY()) > 50 )
return;
if( fabs(psCar1->cPos.GetZ() - psCar2->cPos.GetZ()) > 50 )
return;
// Clear it
for(int i=0;i<16;i++) {
m1[i] = 0;
m2[i] = 0;
}
for(i=0;i<3;i++) {
m1[i] = psCar1->X.GetIndex(i);
m2[i] = psCar2->X.GetIndex(i);
}
for(i=4;i<7;i++) {
m1[i] = psCar1->Y.GetIndex(i-4);
m2[i] = psCar2->Y.GetIndex(i-4);
}
for(i=8;i<11;i++) {
m1[i] = psCar1->Z.GetIndex(i-8);
m2[i] = psCar2->Z.GetIndex(i-8);
}
for(i=12;i<15;i++) {
m1[i] = psCar1->cPos.GetIndex(i-12);
m2[i] = psCar2->cPos.GetIndex(i-12);
}
m1[15] = 1;
m2[15] = 1;
// Rase it a bit
m1[14] += 2;
m2[14] += 2;
CollisionModel3D *col = pcCar1->getColMod();
CollisionModel3D *col2 = pcCar2->getColMod();
col->setTransform( m1 );
//col->setTransform( m2 );
CVec relVel = psCar1->cVelocity - psCar2->cVelocity;
CVec posNorm = psCar1->cPos - psCar2->cPos;
VectorNormalize(&posNorm);
// Check for collision
if(col->collision( col2, -1, 0, m2 )) {
float t1[9], t2[9];
plane_t plane1, plane2;
for(int n=0; n<col->getNumCollisions(); n++) {
// Get collision normal
col->getCollidingTriangles(n,t1,t2,false);
plane2 = CalculatePlane(CVec(t1[0],t1[1],t1[2]),
CVec(t1[3],t1[4],t1[5]),
CVec(t1[6],t1[7],t1[8]));
plane1 = CalculatePlane(CVec(t2[0],t2[1],t2[2]),
CVec(t2[3],t2[4],t2[5]),
CVec(t2[6],t2[7],t2[8]));
//plane1.vNormal.SetZ( 0 );//MIN(plane1.vNormal.GetZ(), 0) );
//plane2.vNormal.SetZ( 0 );//MIN(plane2.vNormal.GetZ(), 0) );
// Make sure we are going into the plane
//if(DotProduct(psCar1->cVelocity, plane1.vNormal) > 0)
//continue;
// Check to make sure the car position is the good side of the plane
//if(DotProduct(psCar1->cPos,plane1.vNormal) + plane1.fDistance < 0)
//continue;
//float nRV = DotProduct(relVel, posNorm);
//if(nRV > 0)
//continue;
break;
}
// Car 1
CVec norm = plane1.vNormal;
float dot = DotProduct(plane1.vNormal, psCar1->cVelocity);
norm = norm * dot;
CVec vt = psCar1->cVelocity - norm;
float coef = 0.01f;
// Ball-on-ball collision
CVec norm2 = norm * coef;
psCar1->cVelocity = vt + norm2;
norm2 = norm * (1.0f - coef);
psCar2->cVelocity += norm2;
// Car 2
/*norm = plane2.vNormal;
dot = DotProduct(plane2.vNormal, psCar2->cVelocity);
norm = norm * dot;
vt = psCar2->cVelocity - norm;
coef = 0.01f;
// Ball-on-ball collision
norm2 = norm * coef;
psCar2->cVelocity = vt + norm2;
norm2 = norm * (1.0f - coef);
psCar1->cVelocity += norm2;*/
}
}
void
CollisionManager::addMesh(MeshPtr pMesh)
{
using ColDet::CollisionModel3D;
if(modelMap.find(pMesh->getName()) != modelMap.end()) return;
unsigned int numVertices = 0;
unsigned int numIndices = 0;
// create a new collision model
CollisionModel3D* mCollisionModel = ColDet::newCollisionModel3D();
unsigned int i = 0, j = 0;
for(i = 0; i < pMesh->getNumSubMeshes(); i++)
{
SubMesh* pSubMesh = pMesh->getSubMesh(i);
if(pSubMesh->vertexData != NULL)
numVertices += pSubMesh->vertexData->vertexCount;
if(pSubMesh->indexData != NULL)
numIndices += pSubMesh->indexData->indexCount;
}
// set the number of triangles
mCollisionModel->setTriangleNumber(numIndices / 3);
// Count the number of vertices and incides so we can Set them
unsigned int indexCount = 0, vertListCount = 0;
for(i = 0; i < pMesh->getNumSubMeshes(); i++)
{
SubMesh* pSubMesh = pMesh->getSubMesh(i);
if(pSubMesh->vertexData != NULL) {
const VertexElement* elem = pSubMesh->vertexData->vertexDeclaration->findElementBySemantic(VES_POSITION);
HardwareVertexBufferSharedPtr vbuf = pSubMesh->vertexData->vertexBufferBinding->getBuffer(elem->getSource());
Real* test = new Real[pSubMesh->vertexData->vertexCount * 3];
float* pDest = test;
unsigned char* pData = static_cast<unsigned char*>(vbuf->lock(HardwareBuffer::HBL_READ_ONLY));
for (j = 0; j < pSubMesh->vertexData->vertexCount; j++) {
float* pFloat;
elem->baseVertexPointerToElement(pData, &pFloat);
*pDest++ = *pFloat++;
*pDest++ = *pFloat++;
*pDest++ = *pFloat++;
pData += vbuf->getVertexSize();
}
vbuf->unlock();
if(pSubMesh->indexData != NULL) {
HardwareIndexBufferSharedPtr ibuf = pSubMesh->indexData->indexBuffer;
unsigned short* test2 = new unsigned short[pSubMesh->indexData->indexCount];
ibuf->readData(0, ibuf->getSizeInBytes(), test2);
float* tri[3];
for (int j = 0; j < pSubMesh->indexData->indexCount; j = j + 3) {
for (int k = 0; k < 3; k++) {
tri[k] = new float[3];
tri[k][0] = test[(test2[j + k] * 3) + 0];
tri[k][1] = test[(test2[j + k] * 3) + 1];
tri[k][2] = test[(test2[j + k] * 3) + 2];
}
mCollisionModel->addTriangle(tri[0], tri[1], tri[2]);
delete[] tri[0];
delete[] tri[1];
delete[] tri[2];
}
delete[] test2;
}
delete[] test;
}
}
mCollisionModel->finalize();
modelMap[pMesh->getName()] = mCollisionModel;
}
///////////////////
// Check collisions between the car & track
void Car_CheckCollisions(carsim_t *psCar, CCar *pcCar, CModel *pcTrack)
{
int i;
float m[16], t1[9], t2[9];
float p[3];
// Clear it
for(i=0;i<16;i++)
m[i] = 0;
for(i=0;i<3;i++)
m[i] = psCar->X.GetIndex(i);
for(i=4;i<7;i++)
m[i] = psCar->Y.GetIndex(i-4);
for(i=8;i<11;i++)
m[i] = psCar->Z.GetIndex(i-8);
for(i=12;i<15;i++)
m[i] = psCar->cPos.GetIndex(i-12);
m[15] = 1;
// Raise the car up a bit
m[14] += 2;
psCar->bCollision = false;
CollisionModel3D *col = pcCar->getColMod();//pcCar->getModel()->GetCDModel();
col->setTransform(m);
if(col->collision( pcTrack->getCDModel(),-1,500 )) {
// Get collision point
//carcol->getCollisionPoint(v1);
//for(i=0;i<3;i++) cont->Pos.SetIndex(i,v1[i]);
//cont->Pos = cont->Pos + offset;
CVec oldVel = psCar->cVelocity;
CVec oldAng = psCar->cAngVelocity;
for(int n=0; n<col->getNumCollisions(); n++) {
// Get collision normal
col->getCollidingTriangles(n,t1,t2,false);
col->getCollisionPoint(n,p,false);
plane_t plane = CalculatePlane(CVec(t2[0],t2[1],t2[2]),
CVec(t2[3],t2[4],t2[5]),
CVec(t2[6],t2[7],t2[8]));
// Resolve the collision
// Check to make sure the plane we're rebounding off is facing opposite the velocity
if(DotProduct(psCar->cVelocity, plane.vNormal) > 0)
continue;
// Check to make sure the car position is the good side of the plane
if(DotProduct(psCar->cPos,plane.vNormal) + plane.fDistance < 0)
continue;
float Mass = 2000;
float coef = 0.2f;
// If the velocity against the mesh is too low, make sure we fully bounce back so we don't slowly
// go into the mesh
if( DotProduct(psCar->cVelocity, plane.vNormal) > -1)
coef = 1.0f;
psCar->bCollision = true;
psCar->cColNormal = plane.vNormal;
psCar->cColPoint = CVec(p[0], p[1], p[2]);
// Linear velocity rebound
float j = DotProduct(psCar->cVelocity * -(1+coef),plane.vNormal) / DotProduct(plane.vNormal,plane.vNormal * (1/Mass));
psCar->cVelocity += plane.vNormal * (j*(1/Mass));
// Angular velocity rebound
float pnt[3];
col->getCollisionPoint(n, pnt);
//CVec p = CVec(pnt[0],pnt[1],pnt[2]);
CVec p = psCar->X*pnt[0] + psCar->Y*pnt[1] + psCar->Z*pnt[2];
CVec v = CrossProduct(oldAng, p) + oldVel;
if(DotProduct(plane.vNormal,v) > EPSILON)
continue;
if(DotProduct(p+psCar->cPos,plane.vNormal) + plane.fDistance > EPSILON)
continue;
v = CrossProduct(p, plane.vNormal*-DotProduct(v,plane.vNormal) * 0.01f);
//psCar->cAngVelocity += v;
}
//rbody.ResolveCollisions();
//return true;
}
}
