void FloatCubeMap_t::Resample( FloatCubeMap_t &out, float flPhongExponent )
{
// terribly slow brute force algorithm just so I can try it out
for(int dface=0;dface<6;dface++)
{
for(int dy=0;dy<out.face_maps[dface].Height;dy++)
for(int dx=0;dx<out.face_maps[dface].Width;dx++)
{
float sum_weights=0;
float sum_rgb[3]={0,0,0};
for(int sface=0;sface<6;sface++)
{
// easy 15% optimization - check if faces point away from each other
if (DotProduct(FaceNormal(sface),FaceNormal(sface))>-0.9)
{
Vector ddir=out.PixelDirection(dface,dx,dy);
for(int sy=0;sy<face_maps[sface].Height;sy++)
for(int sx=0;sx<face_maps[sface].Width;sx++)
{
float dp=DotProduct(ddir,PixelDirection(sface,sx,sy));
if (dp>0.0)
{
dp=pow( dp, flPhongExponent );
sum_weights += dp;
for(int c=0;c<3;c++)
sum_rgb[c] += dp*face_maps[sface].Pixel( sx, sy, c );
}
}
}
}
for(int c=0;c<3;c++)
out.face_maps[dface].Pixel( dx, dy, c )=sum_rgb[c]*(1.0/sum_weights);
}
}
}
//--
//
// ComputeVertexNormals
//
//--
// Compute unit normal of every vertex
void Mesh::ComputeVertexNormals()
{
int i;
// Assume that face normals are computed
assert( FaceNormalNumber() == FaceNumber() );
// Resize and initialize vertex normal array
vertex_normals.assign( VertexNumber(), Vector3d(0,0,0) );
// For every face
for( i=0 ; i<FaceNumber() ; i++ )
{
// Add face normal to vertex normal
VertexNormal(i,0) += FaceNormal(i);
VertexNormal(i,1) += FaceNormal(i);
VertexNormal(i,2) += FaceNormal(i);
}
// For every vertex
for( i=0 ; i<VertexNumber() ; i++)
{
// Normalize vertex normal
VertexNormal(i).Normalize();
}
}
//-----------------------------------------------------------------------------
void SimpleMesh::Initialize(){
// Calculate and store all differentials and area
// First update all face normals and triangle areas
for(unsigned int i = 0; i < mFaces.size(); i++){
mFaces.at(i).normal = FaceNormal(i);
}
// reset the clock
timespec tS;
tS.tv_sec = 0;
tS.tv_nsec = 0;
clock_settime(CLOCK_PROCESS_CPUTIME_ID, &tS);
// Then update all vertex normals and curvature
for(unsigned int i = 0; i < mVerts.size(); i++){
// Vertex normals are just weighted averages
mVerts.at(i).normal = VertexNormal(i);
}
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &tS);
std::cout << "Time for vertnorm calc is: \n" << "sec\n" << tS.tv_nsec / 1000000000.0f << "sec\n";
std::cout << tS.tv_nsec / 1000000.0f << " milliseconds" << std::endl;
// Then update vertex curvature
for(unsigned int i = 0; i < mVerts.size(); i++){
mVerts.at(i).curvature = VertexCurvature(i);
// std::cerr << mVerts.at(i).curvature << "\n";
}
// Finally update face curvature
for(unsigned int i = 0; i < mFaces.size(); i++){
mFaces.at(i).curvature = FaceCurvature(i);
}
}
bool dgCollisionConvexHull::CheckConvex (dgPolyhedra& polyhedra1, const dgBigVector* hullVertexArray) const
{
dgPolyhedra polyhedra(polyhedra1);
dgPolyhedra::Iterator iter (polyhedra);
dgBigVector center (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgInt32 count = 0;
dgInt32 mark = polyhedra.IncLRU();
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
if (edge->m_mark < mark) {
count ++;
center += hullVertexArray[edge->m_incidentVertex];
dgEdge* ptr = edge;
do {
ptr->m_mark = mark;
ptr = ptr->m_twin->m_next;
} while (ptr != edge);
}
}
center = center.Scale3 (dgFloat64 (1.0f) / dgFloat64 (count));
for (iter.Begin(); iter; iter ++) {
dgEdge* const edge = &(*iter);
dgBigVector normal0 (FaceNormal (edge, hullVertexArray));
dgBigVector normal1 (FaceNormal (edge->m_twin, hullVertexArray));
dgBigPlane plane0 (normal0, - (normal0 % hullVertexArray[edge->m_incidentVertex]));
dgBigPlane plane1 (normal1, - (normal1 % hullVertexArray[edge->m_twin->m_incidentVertex]));
dgFloat64 test0 = plane0.Evalue(center);
if (test0 > dgFloat64 (1.0e-3f)) {
return false;
}
dgFloat64 test1 = plane1.Evalue(center);
// if (test1 > dgFloat64 (0.0f)) {
if (test1 > dgFloat64 (1.0e-3f)) {
return false;
}
}
return true;
}
//--
//
// ComputeFaceNormals
//
//--
// Compute unit normal of every faces
void Mesh::ComputeFaceNormals()
{
// Resize face normal array
face_normals.resize( FaceNumber() );
// For every face
for( int i=0; i<FaceNumber(); i++ )
{
// Compute unit face normal
FaceNormal(i) = ComputeFaceNormal(i);
}
}
//-----------------------------------------------------------------------------
bool SimpleMesh::AddFace(const std::vector<Vector3<float> > &verts){
unsigned int ind1, ind2, ind3;
AddVertex(verts.at(0), ind1);
AddVertex(verts.at(1), ind2);
AddVertex(verts.at(2), ind3);
Face tri(ind1, ind2, ind3);
mFaces.push_back(tri);
// Compute and assign a normal
mFaces.back().normal = FaceNormal(mFaces.size() - 1);
return true;
}
/*!
* \param[in] v1 vertex 1, Vector3<float>
* \param[in] v2 vertex 2, Vector3<float>
* \param[in] v3 vertex 3, Vector3<float>
*/
bool HalfEdgeMesh::AddFace(const std::vector<Vector3<float> > &verts){
// Add the vertices of the face/triangle
unsigned int vID0, vID1, vID2;
AddVertex(verts.at(0), vID0);
AddVertex(verts.at(1), vID1);
AddVertex(verts.at(2), vID2);
// Add all half-edge pairs
unsigned int heID0, heID1, heID2, pairID0, pairID1, pairID2;
AddHalfEdgePair(vID0, vID1, heID0, pairID0);
AddHalfEdgePair(vID1, vID2, heID1, pairID1);
AddHalfEdgePair(vID2, vID0, heID2, pairID2);
// Connect inner ring
e(heID0).next = heID1;
e(heID0).prev = heID2;
e(heID1).next = heID2;
e(heID1).prev = heID0;
e(heID2).next = heID0;
e(heID2).prev = heID1;
// Finally, create the face, don't forget to set the normal (which should be normalized)
Face triangle;
triangle.edge = heID0;
mFaces.push_back(triangle);
mFaces.at(mFaces.size()-1).normal = FaceNormal(mFaces.size()-1);
// All half-edges share the same left face (previously added)
unsigned int index = mFaces.size()-1;
e(heID0).face = index;
e(heID1).face = index;
e(heID2).face = index;
// Optionally, track the (outer) boundary half-edges
// to represent non-closed surfaces
return true;
}
Plane AABB::FacePlane(int faceIndex) const
{
assume(0 <= faceIndex && faceIndex <= 5);
return Plane(FaceCenterPoint(faceIndex), FaceNormal(faceIndex));
}
void AABB::Triangulate(int numFacesX, int numFacesY, int numFacesZ,
vec *outPos, vec *outNormal, float2 *outUV,
bool ccwIsFrontFacing) const
{
assume(numFacesX >= 1);
assume(numFacesY >= 1);
assume(numFacesZ >= 1);
assume(outPos);
if (!outPos)
return;
// Generate both X-Y planes.
int i = 0;
for(int face = 0; face < 6; ++face) // Faces run in the order -X, +X, -Y, +Y, -Z, +Z.
{
int numFacesU;
int numFacesV;
bool flip = (face == 1 || face == 2 || face == 5);
if (ccwIsFrontFacing)
flip = !flip;
if (face == 0 || face == 1)
{
numFacesU = numFacesY;
numFacesV = numFacesZ;
}
else if (face == 2 || face == 3)
{
numFacesU = numFacesX;
numFacesV = numFacesZ;
}
else// if (face == 4 || face == 5)
{
numFacesU = numFacesX;
numFacesV = numFacesY;
}
for(int x = 0; x < numFacesU; ++x)
for(int y = 0; y < numFacesV; ++y)
{
float u = (float)x / (numFacesU);
float v = (float)y / (numFacesV);
float u2 = (float)(x+1) / (numFacesU);
float v2 = (float)(y+1) / (numFacesV);
outPos[i] = FacePoint(face, u, v);
outPos[i+1] = FacePoint(face, u, v2);
outPos[i+2] = FacePoint(face, u2, v);
if (flip)
Swap(outPos[i+1], outPos[i+2]);
outPos[i+3] = outPos[i+2];
outPos[i+4] = outPos[i+1];
outPos[i+5] = FacePoint(face, u2, v2);
if (outUV)
{
outUV[i] = float2(u,v);
outUV[i+1] = float2(u,v2);
outUV[i+2] = float2(u2,v);
if (flip)
Swap(outUV[i+1], outUV[i+2]);
outUV[i+3] = outUV[i+2];
outUV[i+4] = outUV[i+1];
outUV[i+5] = float2(u2,v2);
}
if (outNormal)
for(int j = 0; j < 6; ++j)
outNormal[i+j] = FaceNormal(face);
i += 6;
}
}
assert(i == NumVerticesInTriangulation(numFacesX, numFacesY, numFacesZ));
}
bool dgCollisionConvexHull::RemoveCoplanarEdge (dgPolyhedra& polyhedra, const dgBigVector* const hullVertexArray) const
{
bool removeEdge = false;
// remove coplanar edges
dgInt32 mark = polyhedra.IncLRU();
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; ) {
dgEdge* edge0 = &(*iter);
iter ++;
if (edge0->m_incidentFace != -1) {
if (edge0->m_mark < mark) {
edge0->m_mark = mark;
edge0->m_twin->m_mark = mark;
dgBigVector normal0 (FaceNormal (edge0, &hullVertexArray[0]));
dgBigVector normal1 (FaceNormal (edge0->m_twin, &hullVertexArray[0]));
dgFloat64 test = normal0 % normal1;
if (test > dgFloat64 (0.99995f)) {
if ((edge0->m_twin->m_next->m_twin->m_next != edge0) && (edge0->m_next->m_twin->m_next != edge0->m_twin)) {
#define DG_MAX_EDGE_ANGLE dgFloat32 (1.0e-3f)
if (edge0->m_twin == &(*iter)) {
if (iter) {
iter ++;
}
}
dgBigVector e1 (hullVertexArray[edge0->m_twin->m_next->m_next->m_incidentVertex] - hullVertexArray[edge0->m_incidentVertex]);
dgBigVector e0 (hullVertexArray[edge0->m_incidentVertex] - hullVertexArray[edge0->m_prev->m_incidentVertex]);
dgAssert ((e0 % e0) >= dgFloat64 (0.0f));
dgAssert ((e1 % e1) >= dgFloat64 (0.0f));
e0 = e0.Scale3 (dgFloat64 (1.0f) / sqrt (e0 % e0));
e1 = e1.Scale3 (dgFloat64 (1.0f) / sqrt (e1 % e1));
dgBigVector n1 (e0 * e1);
dgFloat64 projection = n1 % normal0;
if (projection >= DG_MAX_EDGE_ANGLE) {
dgBigVector e1 (hullVertexArray[edge0->m_next->m_next->m_incidentVertex] - hullVertexArray[edge0->m_twin->m_incidentVertex]);
dgBigVector e0 (hullVertexArray[edge0->m_twin->m_incidentVertex] - hullVertexArray[edge0->m_twin->m_prev->m_incidentVertex]);
dgAssert ((e0 % e0) >= dgFloat64 (0.0f));
dgAssert ((e1 % e1) >= dgFloat64 (0.0f));
//e0 = e0.Scale3 (dgRsqrt (e0 % e0));
//e1 = e1.Scale3 (dgRsqrt (e1 % e1));
e0 = e0.Scale3 (dgFloat64 (1.0f) / sqrt (e0 % e0));
e1 = e1.Scale3 (dgFloat64 (1.0f) / sqrt (e1 % e1));
dgBigVector n1 (e0 * e1);
projection = n1 % normal0;
if (projection >= DG_MAX_EDGE_ANGLE) {
dgAssert (&(*iter) != edge0);
dgAssert (&(*iter) != edge0->m_twin);
polyhedra.DeleteEdge(edge0);
removeEdge = true;
}
}
} else {
dgEdge* next = edge0->m_next;
dgEdge* prev = edge0->m_prev;
polyhedra.DeleteEdge(edge0);
for (edge0 = next; edge0->m_prev->m_twin == edge0; edge0 = next) {
next = edge0->m_next;
polyhedra.DeleteEdge(edge0);
}
for (edge0 = prev; edge0->m_next->m_twin == edge0; edge0 = prev) {
prev = edge0->m_prev;
polyhedra.DeleteEdge(edge0);
}
iter.Begin();
removeEdge = true;
}
}
}
}
}
return removeEdge;
}
void HalfEdgeMesh::Update() {
// Calculate and store all differentials and area
// First update all face normals and triangle areas
for(unsigned int i = 0; i < GetNumFaces(); i++){
f(i).normal = FaceNormal(i);
}
// Then update all vertex normals and curvature
for(unsigned int i = 0; i < GetNumVerts(); i++){
// Vertex normals are just weighted averages
mVerts.at(i).normal = VertexNormal(i);
}
// Then update vertex curvature
for(unsigned int i = 0; i < GetNumVerts(); i++){
mVerts.at(i).curvature = VertexCurvature(i);
// std::cerr << mVerts.at(i).curvature << "\n";
}
// Finally update face curvature
for(unsigned int i = 0; i < GetNumFaces(); i++){
f(i).curvature = FaceCurvature(i);
}
std::cerr << "Area: " << Area() << ".\n";
std::cerr << "Volume: " << Volume() << ".\n";
// Update vertex and face colors
if (mVisualizationMode == CurvatureVertex) {
std::vector<Vertex>::iterator iter = mVerts.begin();
std::vector<Vertex>::iterator iend = mVerts.end();
float minCurvature = (std::numeric_limits<float>::max)();
float maxCurvature = -(std::numeric_limits<float>::max)();
while (iter != iend) {
if (minCurvature > (*iter).curvature) minCurvature = (*iter).curvature;
if (maxCurvature < (*iter).curvature) maxCurvature = (*iter).curvature;
iter++;
}
std::cerr << "Mapping color based on vertex curvature with range [" << minCurvature << "," << maxCurvature << "]" << std::endl;
iter = mVerts.begin();
while (iter != iend) {
(*iter).color = mColorMap->Map((*iter).curvature, minCurvature, maxCurvature);
iter++;
}
}
else if (mVisualizationMode == CurvatureFace) {
std::vector<Face>::iterator iter = mFaces.begin();
std::vector<Face>::iterator iend = mFaces.end();
float minCurvature = (std::numeric_limits<float>::max)();
float maxCurvature = -(std::numeric_limits<float>::max)();
while (iter != iend) {
if (minCurvature > (*iter).curvature) minCurvature = (*iter).curvature;
if (maxCurvature < (*iter).curvature) maxCurvature = (*iter).curvature;
iter++;
}
std::cerr << "Mapping color based on face curvature with range [" << minCurvature << "," << maxCurvature << "]" << std::endl;
iter = mFaces.begin();
while (iter != iend) {
(*iter).color = mColorMap->Map((*iter).curvature, minCurvature, maxCurvature);
iter++;
}
}
}
void Polyhedron::MergeConvex(const float3 &point)
{
// LOGI("mergeconvex.");
std::set<std::pair<int, int> > deletedEdges;
std::map<std::pair<int, int>, int> remainingEdges;
for(size_t i = 0; i < v.size(); ++i)
if (point.DistanceSq(v[i]) < 1e-3f)
return;
// bool hadDisconnectedHorizon = false;
for(int i = 0; i < (int)f.size(); ++i)
{
// Delete all faces that don't contain the given point. (they have point in their positive side)
Plane p = FacePlane(i);
Face &face = f[i];
if (p.SignedDistance(point) > 1e-5f)
{
bool isConnected = (deletedEdges.empty());
int v0 = face.v.back();
for(size_t j = 0; j < face.v.size() && !isConnected; ++j)
{
int v1 = face.v[j];
if (deletedEdges.find(std::make_pair(v1, v0)) != deletedEdges.end())
{
isConnected = true;
break;
}
v0 = v1;
}
if (isConnected)
{
v0 = face.v.back();
for(size_t j = 0; j < face.v.size(); ++j)
{
int v1 = face.v[j];
deletedEdges.insert(std::make_pair(v0, v1));
// LOGI("Edge %d,%d is to be deleted.", v0, v1);
v0 = v1;
}
// LOGI("Deleting face %d: %s. Distance to vertex %f", i, face.ToString().c_str(), p.SignedDistance(point));
std::swap(f[i], f.back());
f.pop_back();
--i;
continue;
}
// else
// hadDisconnectedHorizon = true;
}
int v0 = face.v.back();
for(size_t j = 0; j < face.v.size(); ++j)
{
int v1 = face.v[j];
remainingEdges[std::make_pair(v0, v1)] = i;
// LOGI("Edge %d,%d is to be deleted.", v0, v1);
v0 = v1;
}
}
// The polyhedron contained our point, nothing to merge.
if (deletedEdges.empty())
return;
// Add the new point to this polyhedron.
// if (!v.back().Equals(point))
v.push_back(point);
/*
// Create a look-up index of all remaining uncapped edges of the polyhedron.
std::map<std::pair<int,int>, int> edgesToFaces;
for(size_t i = 0; i < f.size(); ++i)
{
Face &face = f[i];
int v0 = face.v.back();
for(size_t j = 0; j < face.v.size(); ++j)
{
int v1 = face.v[j];
edgesToFaces[std::make_pair(v1, v0)] = i;
v0 = v1;
}
}
*/
// Now fix all edges by adding new triangular faces for the point.
// for(size_t i = 0; i < deletedEdges.size(); ++i)
for(std::set<std::pair<int, int> >::iterator iter = deletedEdges.begin(); iter != deletedEdges.end(); ++iter)
{
std::pair<int, int> opposite = std::make_pair(iter->second, iter->first);
if (deletedEdges.find(opposite) != deletedEdges.end())
continue;
// std::map<std::pair<int,int>, int>::iterator iter = edgesToFaces.find(deletedEdges[i]);
// std::map<std::pair<int,int>, int>::iterator iter = edgesToFaces.find(deletedEdges[i]);
// if (iter != edgesToFaces.end())
{
// If the adjoining face is planar to the triangle we'd like to add, instead extend the face to enclose
// this vertex.
//float3 newTriangleNormal = (v[v.size()-1]-v[iter->second]).Cross(v[iter->first]-v[iter->second]).Normalized();
std::map<std::pair<int, int>, int>::iterator existing = remainingEdges.find(opposite);
assert(existing != remainingEdges.end());
MARK_UNUSED(existing);
#if 0
int adjoiningFace = existing->second;
if (FaceNormal(adjoiningFace).Dot(newTriangleNormal) >= 0.99999f) ///\todo float3::IsCollinear
{
bool added = false;
Face &adjoining = f[adjoiningFace];
for(size_t i = 0; i < adjoining.v.size(); ++i)
if (adjoining.v[i] == iter->second)
{
adjoining.v.insert(adjoining.v.begin() + i + 1, v.size()-1);
added = true;
/*
int prev2 = (i + adjoining.v.size() - 1) % adjoining.v.size();
int prev = i;
int cur = i + 1;
int next = (i + 2) % adjoining.v.size();
int next2 = (i + 3) % adjoining.v.size();
if (float3::AreCollinear(v[prev2], v[prev], v[cur]))
adjoining.v.erase(adjoining.v.begin() + prev);
else if (float3::AreCollinear(v[prev], v[cur], v[next]))
adjoining.v.erase(adjoining.v.begin() + cur);
else if (float3::AreCollinear(v[cur], v[next], v[next2]))
adjoining.v.erase(adjoining.v.begin() + next2);
*/
break;
}
assert(added);
assume(added);
}
else
#endif
// if (!v[deletedEdges[i].first].Equals(point) && !v[deletedEdges[i].second].Equals(point))
{
Face tri;
tri.v.push_back(iter->second);
tri.v.push_back((int)v.size()-1);
tri.v.push_back(iter->first);
f.push_back(tri);
// LOGI("Added face %d: %s.", (int)f.size()-1, tri.ToString().c_str());
}
}
}
#define mathasserteq(lhs, op, rhs) do { if (!((lhs) op (rhs))) { LOGE("Condition %s %s %s (%g %s %g) failed!", #lhs, #op, #rhs, (double)(lhs), #op, (double)(rhs)); assert(false); } } while(0)
// mathasserteq(NumVertices() + NumFaces(), ==, 2 + NumEdges());
assert(FaceIndicesValid());
// assert(EulerFormulaHolds());
// assert(IsClosed());
// assert(FacesAreNondegeneratePlanar());
// assert(IsConvex());
// if (hadDisconnectedHorizon)
// MergeConvex(point);
}
