void Plane::MakeGizmo()
{
float lineSegmentLength = 300;
glm::vec3 planeNormal(normal.x, normal.y, normal.z);
glm::vec3 parallel1(0, 0, 0);
glm::vec3 parallel2(0, 0, 0);
if (planeNormal == glm::vec3(0, 0, 1))
parallel1 = glm::vec3(0, 1, 0);
else
parallel1 = glm::vec3(0, 0, 1);
//get vectors that are perpendecular to the plan
parallel1 = glm::cross(planeNormal, parallel1);
parallel2 = glm::cross(planeNormal, parallel1);
glm::vec3 centrePoint = planeNormal * normal.w;
parallel1 = glm::normalize(parallel1);
parallel2 = glm::normalize(parallel2);
glm::vec3 start = centrePoint + parallel1 * lineSegmentLength;
glm::vec3 end = centrePoint - parallel1 * lineSegmentLength;
Gizmos::addLine(start, end, color);
start = centrePoint + parallel2 * lineSegmentLength;
end = centrePoint - parallel2 * lineSegmentLength;
Gizmos::addLine(start, end, color);
}
//----------------------------------------------------------------------------
void vesKiwiImageWidgetRepresentation::scrollImageSlice(double deltaX, double deltaY)
{
deltaY *= -1;
vesSharedPtr<vesRenderer> ren = this->renderer();
vesVector3f viewFocus = ren->camera()->focalPoint();
vesVector3f viewFocusDisplay = ren->computeWorldToDisplay(viewFocus);
float focalDepth = viewFocusDisplay[2];
this->Internal->LastTouchPosition += vesVector2f(deltaX, deltaY);
vesVector3f worldTouchPosition = ren->computeDisplayToWorld(vesVector3f(this->Internal->LastTouchPosition[0], this->Internal->LastTouchPosition[1], focalDepth));
worldTouchPosition -= this->Internal->GrabOffset.cast<float>();
int flatDimension = this->Internal->SelectedImageDimension;
vesVector3f planeNormal(0, 0, 0);
planeNormal[flatDimension] = 1.0;
int extent[6];
vesVector3d spacing;
vesVector3d origin;
this->imageData()->GetOrigin(origin.data());
this->imageData()->GetSpacing(spacing.data());
this->imageData()->GetExtent(extent);
double distanceAlongInteractionAxis = worldTouchPosition.dot(planeNormal);
double distanceFromOriginAlongAxis = distanceAlongInteractionAxis - origin[flatDimension];
int slicesFromOrigin = distanceFromOriginAlongAxis / spacing[flatDimension] - extent[flatDimension*2]*spacing[flatDimension];
this->scheduleSetSliceIndex(flatDimension, slicesFromOrigin);
}
void PerformanceTesting::Test(
const std::string & cardir,
const std::string & carname,
ContentManager & content,
std::ostream & info_output,
std::ostream & error_output)
{
info_output << "Beginning car performance test on " << carname << std::endl;
// init track
assert(!track);
assert(!plane);
btVector3 planeNormal(0, 0, 1);
btScalar planeConstant = 0;
plane = new btStaticPlaneShape(planeNormal, planeConstant);
plane->setUserPointer(static_cast<void*>(&surface));
track = new btCollisionObject();
track->setCollisionShape(plane);
track->setActivationState(DISABLE_SIMULATION);
track->setUserPointer(static_cast<void*>(&surface));
world.addCollisionObject(track);
//load the car dynamics
std::tr1::shared_ptr<PTree> cfg;
content.load(cfg, cardir, carname + ".car");
if (!cfg->size())
{
return;
}
// position is the center of a 2 x 4 x 1 meter box on track surface
btVector3 pos(0.0, -2.0, 0.5);
btQuaternion rot = btQuaternion::getIdentity();
const std::string tire = "";
const bool damage = false;
if (!car.Load(*cfg, cardir, tire, pos, rot, damage, world, content, error_output))
{
return;
}
info_output << "Car dynamics loaded" << std::endl;
info_output << carname << " Summary:\n" <<
"Mass (kg) including driver and fuel: " << 1 / car.GetInvMass() << "\n" <<
"Center of mass (m): " << car.GetCenterOfMass() << std::endl;
std::ostringstream statestream;
joeserialize::BinaryOutputSerializer serialize_output(statestream);
if (!car.Serialize(serialize_output))
{
error_output << "Serialization error" << std::endl;
}
//else info_output << "Car state: " << statestream.str();
carstate = statestream.str();
TestMaxSpeed(info_output, error_output);
TestStoppingDistance(false, info_output, error_output);
TestStoppingDistance(true, info_output, error_output);
info_output << "Car performance test complete." << std::endl;
}
cVECTOR3 tFACET<_Mesh_Entities>::Normal() const
{
/* BB * I don't see any division by zero. */
cVECTOR3 planeNormal(0.0, 0.0, 0.0);
const cHALF_EDGE* heInit = HalfEdge();
const cHALF_EDGE* heCurr = heInit;
const cHALF_EDGE* heNext = heCurr->Next();
do {
const cPOINT3& point0 = heCurr->Vertex()->Point();
const cPOINT3& point1 = heNext->Vertex()->Point();
for (INT i = 0 ; i < 3; i++){
INT i1 = i + 1;
if (i1 > 2) i1 -= 3;
INT i2 = i + 2;
if (i2 > 2) i2 -= 3;
planeNormal[i] += (point0[i1]-point1[i1])*(point0[i2]+point1[i2]);
}
heCurr = heNext;
heNext = heNext->Next();
} while(heCurr != heInit);
return planeNormal;
}
float MwMaths::PointToPlaneDistance(MwVector3 &point, MwVector4 &plane)
{
// TODO: sophisticate the straightforward division by plane.z, make conversion functions for plane of Vector3s and of Vector4
MwVector3 planePoint(0.0f, 0.0f, -plane.w / plane.z);
MwVector3 planeNormal(plane.x, plane.y, plane.z);
MwVector3::Normalize(planeNormal, planeNormal);
return -RayToPerpendicularPlaneDistance(point, planeNormal, planePoint);
}
void AutoTriangleMesh<PointType>::calcNormal(const typename AutoTriangleMesh<PointType>::Vertex* vPtr,float normal[3]) const
{
/* Set normal vector to zero: */
for(int i=0;i<3;++i)
normal[i]=0.0f;
/* Iterate through vertex' platelet: */
const Edge* ve=vPtr->getEdge();
do
{
const Edge* ve2=ve->getFacePred()->getOpposite();
float triangleNormal[3];
planeNormal(*ve->getStart(),*ve->getEnd(),*ve2->getEnd(),triangleNormal);
for(int i=0;i<3;++i)
normal[i]+=triangleNormal[i];
/* Go to next edge around vertex: */
ve=ve2;
}
while(ve!=vPtr->getEdge());
}
void ZBspTree::_GetSortedFaceList( D3DXPLANE* pPlane, ZBspFace* pFaces, ZBspFace** fr , ZBspFace** bk )
{
ZBspFace * pFront = NULL, * pBack = NULL, * pNext = NULL;
ZBspFace * pCurrentFace = NULL;
if( !pFaces )
{
*fr = NULL;
*bk = NULL;
return;
}
for( pCurrentFace = pFaces ; pCurrentFace ; pCurrentFace = pFaces )
{
pFaces = pFaces->GetNext();
D3DXVECTOR3 planeNormal( pPlane->a, pPlane->b, pPlane->c );
D3DXVECTOR3 currentNormal( pCurrentFace->GetPlane()->a, pCurrentFace->GetPlane()->b, pCurrentFace->GetPlane()->c );
float val;
int res = ZClassifyByPlane::WhereIsFace( pPlane, pCurrentFace->GetVerts(), pCurrentFace->GetVertCount() );
switch( res )
{
case ZClassifyByPlane::FRONT:
pCurrentFace->AddNext( pFront );
pFront = pCurrentFace;
break;
case ZClassifyByPlane::BACK:
pCurrentFace->AddNext( pBack );
pBack = pCurrentFace;
break;
case ZClassifyByPlane::ON:
pCurrentFace->SetUsed( TRUE );
val = D3DXVec3Dot( &planeNormal, ¤tNormal );
if( val >= 0.0f )
{
pCurrentFace->AddNext( pFront );
pFront = pCurrentFace;
}
else
{
pCurrentFace->AddNext( pBack );
pBack = pCurrentFace;
}
break;
case ZClassifyByPlane::SPLIT:
ZBspFace * front = NULL, * back = NULL;
_Split( pPlane, pCurrentFace, &front, &back );
if( !front && !back )
break;
if( front->GetNext() )
{
front->GetNext()->AddNext( pFront );
pFront = front;
}
else
{
front->AddNext( pFront );
pFront = front;
}
if( back->GetNext() )
{
back->GetNext()->AddNext( pBack );
pBack = back;
}
else
{
back->AddNext( pBack );
pBack = back;
}
// 분할된 face는 새롭게 front, back으로 분할되었으므로
// 메모리에서 반드시 삭제해야한다.
pCurrentFace->AddNext( NULL );
delete pCurrentFace;
break;
}
}
*fr = pFront;
*bk = pBack;
}
void SpheresDemo::setupScene(const ConstructionInfo& ci)
{
if (1)
{
btSphereShape* sphere = new btSphereShape(1);
m_collisionShapes.push_back(sphere);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
float start_x = START_POS_X - ci.gapX*ci.arraySizeX/2;
float start_y = START_POS_Y;
float start_z = START_POS_Z - ci.gapZ*ci.arraySizeZ/2;
for (int k=0;k<ci.arraySizeY;k++)
{
int sizeX = ci.arraySizeX;
int startX = -sizeX/2;
float gapX = ci.gapX;
for (int i=0;i<sizeX;i++)
{
int sizeZ = ci.arraySizeZ;
int startZ = -sizeX/2;
float gapZ =ci.gapZ;
for(int j = 0;j<sizeZ;j++)
{
//btCollisionShape* shape = k==0? boxShape : colShape;
btCollisionShape* shape = sphere;
btScalar mass = 1;
if (!ci.m_useConcaveMesh && k==0)
mass = k==0? 0.f : 1.f;
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
shape->calculateLocalInertia(mass,localInertia);
startTransform.setOrigin(SCALING*btVector3(
btScalar(gapX*i + start_x),
btScalar(ci.gapY*k + start_y),
btScalar(gapZ*j + start_z)));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
m_dynamicsWorld->addRigidBody(body);
}
}
}
}
{
btVector3 planeNormal(0,1,0);
btScalar planeConstant=0;
btCollisionShape* shape = new btStaticPlaneShape(planeNormal,planeConstant);
//btBoxShape* plane = new btBoxShape(btVector3(100,1,100));
//plane->initializePolyhedralFeatures();
//btSphereShape* shape = new btSphereShape(1000);
btScalar mass(0.);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setRotation(btQuaternion(btVector3(1,0,0),0.3));
groundTransform.setOrigin(btVector3(0,0,0));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,shape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
//add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body);
}
}
void VRMeshEditor::renderMesh(VRMeshEditor::DataItem* dataItem) const
{
/* Check if vertex and triangle arrays are large enough: */
unsigned int numVertices=mesh->getNextVertexIndex();
if(dataItem->numVertices<numVertices)
{
delete[] dataItem->vertices;
dataItem->numVertices=numVertices+(numVertices/2);
dataItem->vertices=new MyVertex[dataItem->numVertices];
}
unsigned int numTriangles=mesh->getNumFaces();
if(dataItem->numTriangles<numTriangles)
{
delete[] dataItem->triangles;
dataItem->numTriangles=numTriangles+(numTriangles/2);
dataItem->triangles=new unsigned int[dataItem->numTriangles*3];
}
/* Reset vertex array: */
for(MyMesh::ConstVertexIterator vIt=mesh->beginVertices(); vIt!=mesh->endVertices(); ++vIt)
{
MyVertex* vPtr=&dataItem->vertices[vIt->index];
for(int i=0; i<3; ++i)
{
vPtr->normal[i]=0.0f;
vPtr->position[i]=(*vIt)[i];
}
}
/* Traverse triangles once to calculate normal vectors for smooth shading: */
unsigned int* viPtr=dataItem->triangles;
for(MyMesh::ConstFaceIterator fIt=mesh->beginFaces(); fIt!=mesh->endFaces(); ++fIt,viPtr+=3)
{
/* Gather triangle's points: */
const MyMesh::Vertex* v[3];
const MyMesh::Edge* e=fIt->getEdge();
for(int i=0; i<3; ++i)
{
v[i]=e->getStart();
viPtr[i]=v[i]->index;
e=e->getFaceSucc();
}
/* Calculate triangle's normal vector: */
float normal[3];
planeNormal(*v[0],*v[1],*v[2],normal);
/* Distribute normal vector to triangle's vertices: */
for(int i=0; i<3; ++i)
{
MyVertex* vPtr=&dataItem->vertices[viPtr[i]];
for(int j=0; j<3; ++j)
vPtr->normal[j]+=normal[j];
}
}
#if 0
static bool saveVertices=true;
if(saveVertices)
{
#if 0
float min[3],max[3];
MyMesh::ConstVertexIterator vIt=mesh->beginVertices();
for(int i=0; i<3; ++i)
min[i]=max[i]=vIt->pos()[i];
for(++vIt; vIt!=mesh->endVertices(); ++vIt)
for(int i=0; i<3; ++i)
{
if(min[i]>vIt->pos()[i])
min[i]=vIt->pos()[i];
else if(max[i]<vIt->pos()[i])
max[i]=vIt->pos()[i];
}
float center[3];
for(int i=0; i<3; ++i)
center[i]=(min[i]+max[i])*0.5f;
float scale=60.0f/(max[0]-min[0]);
for(int i=1; i<3; ++i)
{
float scale2=60.0f/(max[i]-min[i]);
if(scale>scale2)
scale=scale2;
}
#endif
#if 0
FILE* sampleFile=fopen("../ScatteredData/ModelVertices.txt","wt");
fprintf(sampleFile,"%u 3 1\n",numVertices);
for(MyMesh::ConstVertexIterator vIt=mesh->beginVertices(); vIt!=mesh->endVertices(); ++vIt)
{
MyVertex* vPtr=&dataItem->vertices[vIt->index];
float pos[3];
for(int i=0; i<3; ++i)
pos[i]=vPtr->position[i];
//pos[i]=(vPtr->position[i]-center[i])*scale+64.0f;
float normLen=sqrtf(vPtr->normal[0]*vPtr->normal[0]+vPtr->normal[1]*vPtr->normal[1]+vPtr->normal[2]*vPtr->normal[2]);
fprintf(sampleFile,"%8.3f %8.3f %8.3f 0 0 0 %6.3f %6.3f %6.3f\n",pos[0],pos[1],pos[2],vPtr->normal[0]/normLen,vPtr->normal[1]/normLen,vPtr->normal[2]/normLen);
}
fclose(sampleFile);
#endif
FILE* triangleFile=fopen("../../Teaching/ECS175/Project2/SubdivisionModel.tris","wt");
fprintf(triangleFile,"color %5.3f, %5.3f, %5.3f\n",0.5f,0.5f,0.5f);
for(MyMesh::ConstFaceIterator fIt=mesh->beginFaces(); fIt!=mesh->endFaces(); ++fIt,viPtr+=3)
{
fprintf(triangleFile,"beginPolygon\n");
/* Gather triangle's points: */
const MyMesh::Vertex* v[3];
const MyVertex* vs[3];
const MyMesh::Edge* e=fIt->getEdge();
for(int i=0; i<3; ++i)
{
v[i]=e->getStart();
vs[i]=&dataItem->vertices[v[i]->index];
e=e->getFaceSucc();
}
float normal[3];
planeNormal(*v[0],*v[1],*v[2],normal);
fprintf(triangleFile,"normal %6.3f, %6.3f, %6.3f\n",normal[0],normal[1],normal[2]);
for(int i=0; i<3; ++i)
{
//fprintf(triangleFile,"normal %6.3f, %6.3f, %6.3f\n",vs[i]->normal[0],vs[i]->normal[1],vs[i]->normal[2]);
fprintf(triangleFile,"vertex %8.3f, %8.3f, %8.3f\n",vs[i]->position[0],vs[i]->position[1],vs[i]->position[2]);
}
fprintf(triangleFile,"end\n");
}
fclose(triangleFile);
saveVertices=false;
}
#endif
/* Traverse triangles again to render: */
glEnableClientState(GL_NORMAL_ARRAY);
glEnableClientState(GL_VERTEX_ARRAY);
glVertexPointer(dataItem->vertices);
glDrawElements(GL_TRIANGLES,numTriangles*3,GL_UNSIGNED_INT,dataItem->triangles);
glDisableClientState(GL_NORMAL_ARRAY);
glDisableClientState(GL_VERTEX_ARRAY);
}
int main()
{
//Specify data directory
//const std::string dataDirectory = "/Users/ervislilaj/Desktop/data/SimudPuteh";
const std::string dataDirectory = "/Users/ervislilaj/Desktop/data/SmallCaveDownsampled";
//Calculate paths
const std::string outputDirectory = dataDirectory + "/output";
const std::string offFile = dataDirectory + "/model.off";
const std::string skeletonFile = dataDirectory + "/model.skel";
const std::string segmentationFile = dataDirectory + "/segmentation.seg";
const std::string segmentationFile2 = dataDirectory + "/segmentation2.seg";
//Read files
typedef std::vector<Triangle> TriangleList;
std::vector<Eigen::Vector3f> vertices;
std::vector<Eigen::Vector3f> newVertices /*(vertices.size()*2)*/;
TriangleList triangles;
std::vector<Eigen::Vector3f> border;
std::vector<CurveSkeleton::Vertex> skeletonVertices;
std::vector<Vertex> gang;
std::vector<Eigen::Vector3f> borderVertices;
std::vector<Eigen::Vector3f> intersectionVertexForTriangulation;
std::vector<IndexedTriangle> triIndices;
std::vector<IndexedTriangle> borderIndices;
std::vector<IndexedTriangle> labelIndices;
std::vector<IndexedTriangle> leerIndices;
std::vector<Eigen::Vector3f> verwe;
std::vector<int32_t> segmentation;
std::vector<int32_t> segmentation2;
std::vector<double> doubleSeg;
std::vector<int> labels;
//step & iteration parameter
int iter = 490;
double step = 0.001;
std::cout << "Loading mesh file..." << std::endl;
ReadOff(offFile, vertices, triangles, triIndices);
std::cout << "Loading skeleton..." << std::endl;
CurveSkeleton* skeleton = LoadCurveSkeleton(skeletonFile.c_str());
std::cout << "Loading segmentation..." << std::endl;
ReadSegmentation(segmentationFile2, segmentation2, skeleton);
//copy
std::vector<Vertex> vec2;
std::cout << "Calculating inverse correspondences..." << std::endl;
//Calculate inverse correspondences
std::vector<unsigned int> meshVertexCorrespondsTo(vertices.size());
int iVert = -1;
for (auto& vert : skeleton->vertices)
{
++iVert;
for (auto c : vert.correspondingOriginalVertices)
{
meshVertexCorrespondsTo[c] = iVert;
}
}
for (int i = 0; i < segmentation2.size(); ++i) {
doubleSeg.push_back(segmentation2[i]);
}
for (int i = 0; i < vertices.size(); ++i) {
Vertex* new_v = new Vertex();
new_v->p = vertices[i];
new_v->c = doubleSeg[meshVertexCorrespondsTo[i]] + 0.5;
newVertices.push_back(vertices[i]);
vec2.push_back(new_v[0]);
}
cgv::math::union_find ufTri((int)vertices.size());
borderIndices.clear();
labels.clear();
labelIndices.clear();
//set neighbors id
for (int i = 0; i < triIndices.size(); ++i)
{
for (int j = 0; j < triIndices.size(); ++j)
{
if(i==j);
else{
if (((triIndices[j].i[0] == triIndices[i].i[0]) && (triIndices[j].i[1] == triIndices[i].i[1])) ||
((triIndices[j].i[1] == triIndices[i].i[0]) && (triIndices[j].i[0] == triIndices[i].i[1])) ||
((triIndices[j].i[1] == triIndices[i].i[0]) && (triIndices[j].i[2] == triIndices[i].i[1])) ||
((triIndices[j].i[2] == triIndices[i].i[0]) && (triIndices[j].i[1] == triIndices[i].i[1])) ||
((triIndices[j].i[2] == triIndices[i].i[0]) && (triIndices[j].i[0] == triIndices[i].i[1])) ||
((triIndices[j].i[0] == triIndices[i].i[0]) && (triIndices[j].i[2] == triIndices[i].i[1])))
{
// neighbor 1
triIndices[i].n[0] = &triIndices[j];
}
if (((triIndices[j].i[0] == triIndices[i].i[1]) && (triIndices[j].i[1] == triIndices[i].i[2])) ||
((triIndices[j].i[1] == triIndices[i].i[1]) && (triIndices[j].i[0] == triIndices[i].i[2])) ||
((triIndices[j].i[1] == triIndices[i].i[1]) && (triIndices[j].i[2] == triIndices[i].i[2])) ||
((triIndices[j].i[2] == triIndices[i].i[1]) && (triIndices[j].i[1] == triIndices[i].i[2])) ||
((triIndices[j].i[2] == triIndices[i].i[1]) && (triIndices[j].i[0] == triIndices[i].i[2])) ||
((triIndices[j].i[0] == triIndices[i].i[1]) && (triIndices[j].i[2] == triIndices[i].i[2])))
{
// neighbor 2
triIndices[i].n[1] = &triIndices[j];
}
if (((triIndices[j].i[0] == triIndices[i].i[2]) && (triIndices[j].i[1] == triIndices[i].i[0])) ||
((triIndices[j].i[1] == triIndices[i].i[2]) && (triIndices[j].i[0] == triIndices[i].i[0])) ||
((triIndices[j].i[1] == triIndices[i].i[2]) && (triIndices[j].i[2] == triIndices[i].i[0])) ||
((triIndices[j].i[2] == triIndices[i].i[2]) && (triIndices[j].i[1] == triIndices[i].i[0])) ||
((triIndices[j].i[2] == triIndices[i].i[2]) && (triIndices[j].i[0] == triIndices[i].i[0])) ||
((triIndices[j].i[0] == triIndices[i].i[2]) && (triIndices[j].i[2] == triIndices[i].i[0])))
{
// neighbor 3
triIndices[i].n[2] = &triIndices[j];
}
}
}
}
/*
std::vector<IndexedTriangle> toShow;
for(int i = 0; i < triIndices.size(); i += 50)
{
toShow.push_back(triIndices[i]);
toShow.push_back(*triIndices[i].n[0]);
toShow.push_back(*triIndices[i].n[1]);
toShow.push_back(*triIndices[i].n[2]);
}
*/
borderIndices.clear();
labels.clear();
labelIndices.clear();
for (int i = 0; i < vec2.size(); ++i) //für jeden Vertex
{
for (int j = 0; j < triIndices.size(); ++j) //für jedes Dreieck
{
if ((i == triIndices[j].i[0] || i == triIndices[j].i[1] || i == triIndices[j].i[2]))
{
vec2[i].NeigborTri.push_back(j);
}
}
}
int nrOptimization = 0;
double tmpLengthIter = 0;
double lengthIter = 0;
bool firstTimeIter = false;
do
{
std::ofstream fout("iter/50iter" + std::to_string(nrOptimization) + ".xyz", std::ofstream::out);
if(!firstTimeIter)
tmpLengthIter = 99999999;
tmpLengthIter = lengthIter;
lengthIter = 0;
// 1 richtung
for (int i = 0; i < vec2.size(); ++i) //für jeden Vertex
{
double inzidenzEdgesLength = 0;
double tmp = 0;
int index = 0;
do {
Edge e;
tmp = inzidenzEdgesLength;
if (index == 0)
tmp = 10000;
inzidenzEdgesLength = 0;
for (int j = 0; j < vec2[i].NeigborTri.size(); ++j) //für jedes Dreieck
{
triIndices[vec2[i].NeigborTri[j]].visible = true;
if ((i == triIndices[vec2[i].NeigborTri[j]].i[0] || i == triIndices[vec2[i].NeigborTri[j]].i[1] || i == triIndices[vec2[i].NeigborTri[j]].i[2]))
{
//Kanten rechnen sowie deren länge
if (ComputePointsBetweenEdges_v1(vec2, triIndices[vec2[i].NeigborTri[j]], e))
{
e.calculateLength();
inzidenzEdgesLength += e.length;
}
}
}
index++;
vec2[i].c += step;
} while (tmp > inzidenzEdgesLength );
vec2[i].c -= step;
vec2[i].edgeLength = inzidenzEdgesLength;
}
// 2 richtung
for (int i = 0; i < vec2.size(); ++i) //für jeden Vertex
{
double inzidenzEdgesLength = 0;
double tmp = 0;
int index = 0;
do {
Edge e;
tmp = inzidenzEdgesLength;
if (index == 0)
tmp = vec2[i].edgeLength;
inzidenzEdgesLength = 0;
for (int j = 0; j < vec2[i].NeigborTri.size(); ++j) //für jedes Dreieck
{
triIndices[vec2[i].NeigborTri[j]].visible = true;
if ((i == triIndices[vec2[i].NeigborTri[j]].i[0] || i == triIndices[vec2[i].NeigborTri[j]].i[1] || i == triIndices[vec2[i].NeigborTri[j]].i[2]))
{
//Kanten rechnen sowie deren länge
if (ComputePointsBetweenEdges_v1(vec2, triIndices[vec2[i].NeigborTri[j]], e))
{
e.calculateLength();
inzidenzEdgesLength += e.length;
}
}
}
//edgeLengthBlock = inzidenzEdgesLength
index++;
vec2[i].c -= step;
lengthIter += inzidenzEdgesLength;
} while (tmp > inzidenzEdgesLength );
vec2[i].c += step;
}
for (int j = 0; j < triIndices.size(); ++j) //für jedes Dreieck
{
Edge e2;
if (ComputePointsBetweenEdges_v1(vec2, triIndices[j], e2)) {
fout << e2.a.x() << " " << e2.a.y() << " " << e2.a.z() << std::endl;
fout << e2.b.x() << " " << e2.b.y() << " " << e2.b.z() << std::endl;
}
}
nrOptimization++;
fout.close();
std::cout << nrOptimization << std::endl;
std::cout << lengthIter << std::endl;
} while ( (nrOptimization < iter ) && ((tmpLengthIter >= lengthIter) || (nrOptimization < 450)) );
/*End Optimization*/
borderVertices.clear();
for (int j = 0; j < triIndices.size(); ++j) //für jedes Dreieck
{
triIndices[j].iD = j;
Edge e2;
if (ComputePointsBetweenEdges_v1(vec2, triIndices[j], e2)) {
borderVertices.push_back(e2.a);
borderVertices.push_back(e2.b);
}
}
// übergang erkennen
for (int i = 0; i < triIndices.size(); ++i) {
auto v0 = vec2.begin() + (triIndices[i].i[0]);
auto v1 = vec2.begin() + (triIndices[i].i[1]);
auto v2 = vec2.begin() + (triIndices[i].i[2]);
double first = v0[0].c;
double second = v1[0].c;
double third = v2[0].c;
if ((first >= 0.f && second < 0.f) || (first < 0.f && second >= 0.f) || (third >= 0.f && second < 0.f) || (third < 0.f && second >= 0.f) || (third >= 0.f && first < 0.f) || (third < 0.f && first >= 0.f)) {
IndexedTriangle t;
t.i[0] = triIndices[i].i[0];
t.i[1] = triIndices[i].i[1];
t.i[2] = triIndices[i].i[2];
t.iD = triIndices[i].iD;
t.n[0] = triIndices[i].n[0];
t.n[1] = triIndices[i].n[1];
t.n[2] = triIndices[i].n[2];
borderIndices.push_back(t);
}
}
//schallen unite
for (int i = 0; i < borderIndices.size(); ++i)
for (int j = 0; j < borderIndices.size(); ++j)
{
if ((borderIndices[i].i[0] == borderIndices[j].i[0]) || (borderIndices[i].i[0] == borderIndices[j].i[1]) || (borderIndices[i].i[0] == borderIndices[j].i[2]))
{
ufTri.unite(i, j);
}
if ((borderIndices[i].i[1] == borderIndices[j].i[0]) || (borderIndices[i].i[1] == borderIndices[j].i[1]) || (borderIndices[i].i[1] == borderIndices[j].i[2]))
{
ufTri.unite(i, j);
}
if ((borderIndices[i].i[2] == borderIndices[j].i[0]) || (borderIndices[i].i[2] == borderIndices[j].i[1]) || (borderIndices[i].i[2] == borderIndices[j].i[2]))
{
ufTri.unite(i, j);
}
}
std::map<int, std::vector<IndexedTriangle>> labelmain;
// in LabelIndices 1 eiziger gang speichern
for (int i = 0; i < borderIndices.size(); ++i)
{
if (ufTri.num_in_set(ufTri.find(i)) <= 1);
else {
if (std::find(labels.begin(), labels.end(), ufTri.find(i)) != labels.end());
else {
labels.push_back(ufTri.find(i));
}
}
if (labels.size() != 0)
{
int key = ufTri.find(i);
labelmain[key].push_back(borderIndices[i]);
}
}
std::vector<IndexedTriangle> newTriangles;
int offset = (int)vec2.size();
for (int label : labels)
{
labelIndices = labelmain[label];
std::vector<Eigen::Vector3f> labelVertices;
std::vector<Eigen::Vector3f> halleVertices;
labelVertices.clear();
Edge labelEdge;
for (int i = 0; i < labelIndices.size(); ++i)
{
if (ComputePointsBetweenEdges_v1(vec2, labelIndices[i], labelEdge))
{
labelVertices.push_back(labelEdge.a);
labelVertices.push_back(labelEdge.b);
}
}
/*Plane Fitting with SVD */
Eigen::Vector3f vor_c(0, 0, 0);
Eigen::Vector3f c(0, 0, 0);
Eigen::MatrixXf matA(3, labelVertices.size());
// calculate c
for (int i = 0; i < labelVertices.size(); ++i)
{
vor_c += labelVertices[i];
}
c = vor_c / labelVertices.size();
//fill the Matrix
for (int i = 0; i < labelVertices.size(); i++)
{
float xB, yB, zB;
xB = labelVertices[i].x() - c.x();
yB = labelVertices[i].y() - c.y();
zB = labelVertices[i].z() - c.z();
matA.col(i) << xB, yB, zB;
}
Eigen::JacobiSVD<Eigen::MatrixXf> svd(matA, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::Vector3f planeNormal(0, 0, 0);
planeNormal = svd.matrixU().col(2);
float d = -(planeNormal.x()*c.x() + planeNormal.y()*c.y() + planeNormal.z()*c.z());
/*Begin Normal direction of the plane*/
std::vector<Eigen::Vector3f> halleVerticesSidePLane;
std::vector<double> disthalleVerticesSidePLane;
int indexHalleV;
Eigen::Vector3f halle_cVector;
//alle halle knoten hinzufügen
for(int i = 0; i < labelIndices.size(); ++i)
{
if(vec2[labelIndices[i].i[0]].c >= 0)
halleVerticesSidePLane.push_back(vec2[labelIndices[i].i[0]].p);
if(vec2[labelIndices[i].i[1]].c >= 0)
halleVerticesSidePLane.push_back(vec2[labelIndices[i].i[1]].p);
if(vec2[labelIndices[i].i[2]].c >= 0)
halleVerticesSidePLane.push_back(vec2[labelIndices[i].i[2]].p);
}
for(int i = 0; i < halleVerticesSidePLane.size(); ++i)
{
disthalleVerticesSidePLane.push_back( Dist2Plane(halleVerticesSidePLane[i], planeNormal, d));
}
indexHalleV = *std::max_element(disthalleVerticesSidePLane.begin(), disthalleVerticesSidePLane.end());
halle_cVector = halleVerticesSidePLane[indexHalleV] - c;
Eigen::Vector3f diffBetweenPlane_halle_positive = halle_cVector + planeNormal;
Eigen::Vector3f diffBetweenPlane_halle_negative = halle_cVector - planeNormal;
float lengthPositive =sqrt(diffBetweenPlane_halle_positive.x()* diffBetweenPlane_halle_positive.x() + diffBetweenPlane_halle_positive.y()* diffBetweenPlane_halle_positive.y() + diffBetweenPlane_halle_positive.z()* diffBetweenPlane_halle_positive.z() );
float lengthNegative =sqrt(diffBetweenPlane_halle_negative.x()* diffBetweenPlane_halle_negative.x() + diffBetweenPlane_halle_negative.y()* diffBetweenPlane_halle_negative.y() + diffBetweenPlane_halle_negative.z()* diffBetweenPlane_halle_negative.z() );
if (lengthPositive > lengthNegative)
planeNormal = -planeNormal;
else if(lengthPositive <= lengthNegative)
planeNormal = planeNormal;
// ax + bx + cx + d = 0 Plane equation
d = -(planeNormal.x()*c.x() + planeNormal.y()*c.y() + planeNormal.z()*c.z());
/*Begin computing Intersection Points*/
//Point which has the largest distance to c
std::vector<double> MaxDistanceToC;
double radiusMax;
for (int i = 0; i < labelVertices.size(); ++i)
{
MaxDistanceToC.push_back(Dist2Points(c, labelVertices[i]));
}
radiusMax = *std::max_element(MaxDistanceToC.begin(), MaxDistanceToC.end());
//Computing Intersection points
int succesor = offset - 1;
std::vector<Edge> intersectionEdges;
intersectionEdges.clear();
//Label Nachbarn hinzufügen. 8schritte 3^12
// First Triangle Hinzufügen die geschnitten wird und teil von lableindices ist
IndexedTriangle firstTri;
int firstTriID = 0;
bool firstTimeSearch = true;
for (int i = 0; i < labelIndices.size(); ++i){
Eigen::Vector3f aP, bP, cP, intersection1, intersection2;
int aI = 0, bI = 0, cI = 0;
aI = labelIndices[i].i[0];
bI = labelIndices[i].i[1];
cI = labelIndices[i].i[2];
aP = vec2[aI].p;
bP = vec2[bI].p;
cP = vec2[cI].p;
if((SegPlaneIntersection(aP, bP, intersection1, planeNormal, d) || SegPlaneIntersection(aP, cP, intersection2, planeNormal, d) ||
SegPlaneIntersection(bP, cP, intersection2, planeNormal, d)) && firstTimeSearch)
{
firstTri = triIndices[labelIndices[i].iD];
firstTimeSearch = false;
firstTriID = labelIndices[i].iD;
}
}
std::set<int> triIndicesHit ;
int mico = 0;
do{
for (int m = 0; m < 3 ; ++m)
{
Eigen::Vector3f aP, bP, cP, intersection1, intersection2;
IndexedTriangle t1,t2;
//indices
int aI = firstTri.n[m]->i[0];
int bI = firstTri.n[m]->i[1];
int cI = firstTri.n[m]->i[2];
aP = vec2[aI].p;
bP = vec2[bI].p;
cP = vec2[cI].p;
if((SegPlaneIntersection(aP, bP, intersection1, planeNormal, d) || SegPlaneIntersection(aP, cP, intersection2, planeNormal, d) ||
SegPlaneIntersection(bP, cP, intersection2, planeNormal, d)) && (triIndicesHit.find(firstTri.n[m]->iD) == triIndicesHit.end())){
triIndicesHit.insert(firstTri.n[m]->iD);
if (SegPlaneIntersection(aP, bP, intersection1, planeNormal, d) && SegPlaneIntersection(aP, cP, intersection2, planeNormal, d) && (Dist2Plane(aP, planeNormal, d) < 0))
{
newVertices.push_back(intersection1);
newVertices.push_back(intersection2);
t1.i[0] = aI;
t1.i[2] = succesor + 2;
t1.i[1] = succesor + 1;
t1.visible = true;
newTriangles.push_back(t1);
firstTri = *firstTri.n[m];
succesor++;
succesor++;
}
if (SegPlaneIntersection(bP, cP, intersection1, planeNormal, d) && SegPlaneIntersection(bP, aP, intersection2, planeNormal, d) && (Dist2Plane(bP, planeNormal, d) < 0))
{
newVertices.push_back(intersection1);
newVertices.push_back(intersection2);
t1.i[0] = bI;
t1.i[2] = succesor + 2;
t1.i[1] = succesor + 1;
t1.visible = true;
newTriangles.push_back(t1);
firstTri = *firstTri.n[m];
succesor++;
succesor++;
}
if (SegPlaneIntersection(cP, aP, intersection1, planeNormal, d) && SegPlaneIntersection(cP, bP, intersection2, planeNormal, d) && (Dist2Plane(cP, planeNormal, d) < 0))
{
newVertices.push_back(intersection1);
newVertices.push_back(intersection2);
t1.i[0] = cI;
t1.i[2] = succesor + 2;
t1.i[1] = succesor + 1;
t1.visible = true;
newTriangles.push_back(t1);
firstTri = *firstTri.n[m];
succesor++;
succesor++;
}
if (SegPlaneIntersection(aP, bP, intersection1, planeNormal, d) && SegPlaneIntersection(aP, cP, intersection2, planeNormal, d) && (Dist2Plane(bP, planeNormal, d) < 0) && (Dist2Plane(cP, planeNormal, d) < 0))
{
newVertices.push_back(intersection1);
newVertices.push_back(intersection2);
t1.i[0] = bI;
t1.i[2] = succesor + 1;
t1.i[1] = succesor + 2;
t2.i[0] = cI;
t2.i[2] = bI;
t2.i[1] = succesor + 2;
t1.visible = true;
t2.visible = true;
newTriangles.push_back(t1);
newTriangles.push_back(t2);
firstTri = *firstTri.n[m];
succesor++;
succesor++;
}
if (SegPlaneIntersection(bP, cP, intersection1, planeNormal, d) && SegPlaneIntersection(bP, aP, intersection2, planeNormal, d) && (Dist2Plane(cP, planeNormal, d) < 0) && (Dist2Plane(aP, planeNormal, d) < 0))
{
newVertices.push_back(intersection1);
newVertices.push_back(intersection2);
t1.i[0] = cI;
t1.i[2] = succesor + 1;
t1.i[1] = succesor + 2;
t2.i[0] = aI;
t2.i[2] = cI;
t2.i[1] = succesor + 2;
t1.visible = true;
t2.visible = true;
newTriangles.push_back(t1);
newTriangles.push_back(t2);
firstTri = *firstTri.n[m];
succesor++;
succesor++;
}
if (SegPlaneIntersection(cP, aP, intersection1, planeNormal, d) && SegPlaneIntersection(cP, bP, intersection2, planeNormal, d) && (Dist2Plane(aP, planeNormal, d) < 0) && (Dist2Plane(bP, planeNormal, d) < 0))
{
newVertices.push_back(intersection1);
newVertices.push_back(intersection2);
t1.i[0] = aI;
t1.i[2] = succesor + 1;
t1.i[1] = succesor + 2;
t2.i[0] = bI;
t2.i[2] = aI;
t2.i[1] = succesor + 2;
t1.visible = true;
t2.visible = true;
newTriangles.push_back(t1);
newTriangles.push_back(t2);
firstTri = *firstTri.n[m];
succesor++;
succesor++;
}
}
}
mico ++;
}while (mico < 1000000);
//nachbarn von Schnitt hinzufügen
std::set<int> extendedLabelIndices ;
for (int i = 0; i < triIndices.size(); ++i)
{
const bool is_in = triIndicesHit.find(triIndices[i].iD) != triIndicesHit.end();
if(is_in){
extendedLabelIndices.insert(triIndices[i].iD);
for(int j = 0; j < 3; ++j){
extendedLabelIndices.insert(triIndices[i].n[j]->iD);
for(int k = 0; k < 3; ++k){
extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->iD);
for(int l = 0; l < 3; ++l){
extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->n[l]->iD);
for(int f = 0; f < 3; ++f){
extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->n[l]->n[f]->iD);
for(int s = 0; s < 3; ++s){
extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->n[l]->n[f]->n[s]->iD);
for(int r = 0; r < 3; ++r){
extendedLabelIndices.insert(triIndices[i].n[j]->n[k]->n[l]->n[f]->n[s]->n[r]->iD);
}
}
}
}
}
}
}
}
/* for (int i = 0; i < triIndices.size(); ++i)
{
int a = triIndices[i].i[0],b = triIndices[i].i[1] ,c = triIndices[i].i[2] ;
if(( vec2[a].c >= 0 )&& ( vec2[b].c >= 0 ) && ( vec2[c].c >= 0 ))
{
triIndices[i].visible = true;
}
}*/
for (int i = 0; i < triIndices.size(); ++i)
{
//positions
Eigen::Vector3f aP, bP, cP, intersection1, intersection2;
//distance to middlepoint
float dista, distb, distc;
//indices
int aI = 0, bI = 0, cI = 0;
aI = triIndices[i].i[0];
bI = triIndices[i].i[1];
cI = triIndices[i].i[2];
aP = vec2[aI].p;
bP = vec2[bI].p;
cP = vec2[cI].p;
dista = Dist2Points(c, aP);
distb = Dist2Points(c, bP);
distc = Dist2Points(c, cP);
const bool is_in = extendedLabelIndices.find(triIndices[i].iD) != extendedLabelIndices.end();
if (is_in)
{
if ((Dist2Plane(aP, planeNormal, d) > 0) || (Dist2Plane(bP, planeNormal, d) > 0) || (Dist2Plane(cP, planeNormal, d) > 0))
{
triIndices[i].visible = false;
}
if (((Dist2Plane(aP, planeNormal, d) <= 0) || (Dist2Plane(bP, planeNormal, d) <= 0) || (Dist2Plane(cP, planeNormal, d) <= 0)) && ((Dist2Plane(aP, planeNormal, d) > 10) || (Dist2Plane(bP, planeNormal, d) > 10) || (Dist2Plane(cP, planeNormal, d) > 10)))
{
triIndices[i].visible = true;
}
}
}
//umbrella
Eigen::Vector3f vor_ca(0,0,0), ca(0, 0, 0);
int anz = (int)newVertices.size() - (offset);
for (int j = offset ; j < (int)newVertices.size() ; ++j)
{
vor_ca += newVertices[j];
}
ca = vor_c / anz;
newVertices.push_back(c);
for (int j = offset; j < newVertices.size()-2; j+=2)
{
IndexedTriangle t;
t.i[1] = j;
t.i[2] = j + 1;
t.i[0] = (int)newVertices.size() - 1;
newTriangles.push_back(t);
}
offset = (int)newVertices.size();
/*End computing Intersection Points*/
}
for(auto t : triIndices)
if(t.visible && (vec2[t.i[0]].c >= 0 || vec2[t.i[1]].c >= 0 || vec2[t.i[2]].c >= 0))
newTriangles.push_back(t);
WriteSegmentation(segmentationFile, segmentation2);
ReadSegmentation(segmentationFile, segmentation, skeleton);
//Test output
std::cout << "Writing output file..." << std::endl;
/*int colors[10][3] =
{
{ 166, 206, 227 },
{ 31, 120, 180 },
{ 251, 154, 153 },
{ 227, 26, 28 },
{ 253, 191, 111 },
{ 255, 127, 0 },
{ 202, 178, 214 },
{ 106, 61, 154 },
{ 255, 255, 153 },
{ 177, 89, 40 }
};*/
auto colorFunc = [&](int i, int& r, int& g, int& b)
{
if (doubleSeg[i] == -1)
{
//color for passages
r = 0; g = 175; b = 0;
}
else
if (doubleSeg[i] >= 0)
{
r = 166; g = 175; b = 227;
}
else
if (doubleSeg[i] == 0)
{
r = 0; g = 0; b = 175;
}
};
std::string segmentedMeshFile = outputDirectory + "/segmentedMesh.off";
WriteOff(segmentedMeshFile.c_str(), vertices, triIndices, [&](int i, int& r, int& g, int& b) {colorFunc(meshVertexCorrespondsTo[i], r, g, b); });
std::string segmentedMeshFile2 = outputDirectory + "/borderVertices.off";
WriteOff(segmentedMeshFile2.c_str(), vertices, borderIndices, [&](int i, int& r, int& g, int& b) {colorFunc(meshVertexCorrespondsTo[i], r, g, b); });
std::string segmentedMeshFile3 = outputDirectory + "/schalle.off";
WriteOff(segmentedMeshFile3.c_str(), vertices, borderIndices, [&](int i, int& r, int& g, int& b) {colorFunc(meshVertexCorrespondsTo[i], r, g, b); });
std::string segmentedMeshFile7 = outputDirectory + "/marchingBig.off";
WriteOff(segmentedMeshFile7.c_str(), verwe , leerIndices, [&](int i, int& r, int& g, int& b) { r = 175; g = 0; b = 0; });
std::string segmentedMeshFile8 = outputDirectory + "/new.off";
WriteOff(segmentedMeshFile8.c_str(), newVertices, newTriangles, [&](int i, int& r, int& g, int& b) { r = 200; g = 200; b = 0; });
//system("PAUSE");
}
void testConvexHullProjectionWithGravity()
{
// For this test, we will use a simple convex hull of side 1 meter
// centered in the origin of the world and laying on the XY plane
iDynTree::ConvexHullProjectionConstraint projectionConstraint;
iDynTree::Polygon convexHull;
convexHull.m_vertices.push_back(iDynTree::Position( 0.5, 0.5, 0.0));
convexHull.m_vertices.push_back(iDynTree::Position(-0.5, 0.5, 0.0));
convexHull.m_vertices.push_back(iDynTree::Position(-0.5, -0.5, 0.0));
convexHull.m_vertices.push_back(iDynTree::Position( 0.5, -0.5, 0.0));
std::vector<iDynTree::Polygon> polygons;
polygons.resize(1);
polygons[0] = convexHull;
// The convex hull is already expressed in the world
std::vector<iDynTree::Transform> transforms;
transforms.push_back(iDynTree::Transform::Identity());
iDynTree::Direction xAxis(1.0, 0.0, 0.0);
iDynTree::Direction yAxis(0.0, 1.0, 0.0);
bool ok = projectionConstraint.buildConvexHull(xAxis,yAxis,iDynTree::Position::Zero(),polygons,transforms);
ASSERT_IS_TRUE(ok);
// Use a test position that is outside the convex hull with the normal projection
iDynTree::Position testPoint(1.0, 0.0, 1.0);
// Projected along the normal of the plane, this point is outside the convex hull
// Note: positive margin means inside, negative outside
ASSERT_IS_TRUE(projectionConstraint.computeMargin(projectionConstraint.project(testPoint)) < 0);
//----------------------------------------------------------------------------------------------
// If the direction along which we are projecting is the normal of the plane, the projection
// should match the one done without direction
iDynTree::Direction planeNormal(0.0, 0.0, -1.0);
projectionConstraint.setProjectionAlongDirection(planeNormal);
ASSERT_EQUAL_VECTOR(projectionConstraint.project(testPoint), projectionConstraint.projectAlongDirection(testPoint));
//----------------------------------------------------------------------------------------------
// If the direction of the projection is "skewed" to the left, the point should instead be inside the convex hull
iDynTree::Direction skewedDirection(-1.0, 0.0, -1.0);
projectionConstraint.setProjectionAlongDirection(skewedDirection);
std::cerr << projectionConstraint.computeMargin(projectionConstraint.projectAlongDirection(testPoint)) << std::endl;
ASSERT_IS_TRUE(projectionConstraint.computeMargin(projectionConstraint.projectAlongDirection(testPoint)) > 0);
//----------------------------------------------------------------------------------------------
// The code should work even if I try to project in the opposite direction wrt 'planeNormal'
iDynTree::Direction upwardDirection(0.0, 0.0, 1.0);
projectionConstraint.setProjectionAlongDirection(upwardDirection);
// I should obtain the same result as the orthogonal projection
ASSERT_EQUAL_VECTOR(projectionConstraint.project(testPoint), projectionConstraint.projectAlongDirection(testPoint));
// The projected point should be again outside the convex hull
ASSERT_IS_TRUE(projectionConstraint.computeMargin(projectionConstraint.projectAlongDirection(testPoint))< 0);
}
void CCampathDrawer::OnPostRenderAllTools()
{
// Actually we are often called twice per frame due to an engine bug(?), once after 3d skybox
// and once after world is drawn, maybe we will be even called more times,
// but we can not care about that for now.
if(!m_Draw)
return;
if(!m_VertexShader)
{
m_VertexShader = g_AfxShaders.GetVertexShader("afx_line_vs20.fxo");
}
IDirect3DVertexShader9 * vertexShader = m_VertexShader->GetVertexShader();
if(!m_PixelShader)
{
m_PixelShader = g_AfxShaders.GetPixelShader("afx_line_ps20.fxo");
}
IDirect3DPixelShader9 * pixelShader = m_PixelShader->GetPixelShader();
if(!(m_Device && vertexShader && m_PixelShader && g_VEngineClient))
{
static bool firstError = true;
if(firstError)
{
firstError = false;
Tier0_Msg(
"AFXERROR: CCampathDrawer::OnEndScene: Missing required dependencies:%s%s%s%s.\n",
!m_Device ? " m_Device" : "",
!vertexShader ? " vertexShader" : "",
!pixelShader ? " pixelShader" : "",
!g_VEngineClient ? " g_VEngineClient" : ""
);
}
return;
}
// Save device state:
IDirect3DPixelShader9 * oldPixelShader = 0;
m_Device->GetPixelShader(&oldPixelShader);
if(oldPixelShader) oldPixelShader->AddRef();
IDirect3DVertexShader9 * oldVertexShader = 0;
m_Device->GetVertexShader(&oldVertexShader);
if(oldVertexShader) oldVertexShader->AddRef();
IDirect3DVertexBuffer9 * oldVertexBuffer = 0;
UINT oldVertexBufferOffset;
UINT oldVertexBufferStride;
m_Device->GetStreamSource(0, &oldVertexBuffer, &oldVertexBufferOffset, &oldVertexBufferStride);
// this is done already according to doc: // if(oldVertexBuffer) oldVertexBuffer->AddRef();
IDirect3DIndexBuffer9 * oldIndexBuffer = 0;
m_Device->GetIndices(&oldIndexBuffer);
// this is done already according to doc: // if(oldIndexBuffer) oldIndexBuffer->AddRef();
IDirect3DVertexDeclaration9 * oldDeclaration;
m_Device->GetVertexDeclaration(&oldDeclaration);
if(oldDeclaration) oldDeclaration->AddRef();
DWORD oldFVF;
m_Device->GetFVF(&oldFVF);
FLOAT oldCViewProj[4][4];
m_Device->GetVertexShaderConstantF(8, oldCViewProj[0], 4);
FLOAT oldCScreenInfo[4];
m_Device->GetVertexShaderConstantF(48, oldCScreenInfo, 1);
FLOAT oldCPlane0[4];
m_Device->GetVertexShaderConstantF(49, oldCPlane0, 1);
FLOAT oldCPlaneN[4];
m_Device->GetVertexShaderConstantF(50, oldCPlaneN, 1);
DWORD oldSrgbWriteEnable;
m_Device->GetRenderState(D3DRS_SRGBWRITEENABLE, &oldSrgbWriteEnable);
DWORD oldColorWriteEnable;
m_Device->GetRenderState(D3DRS_COLORWRITEENABLE, &oldColorWriteEnable);
DWORD oldZEnable;
m_Device->GetRenderState(D3DRS_ZENABLE, &oldZEnable);
DWORD oldZWriteEnable;
m_Device->GetRenderState(D3DRS_ZWRITEENABLE, &oldZWriteEnable);
DWORD oldZFunc;
m_Device->GetRenderState(D3DRS_ZFUNC, &oldZFunc);
DWORD oldAlphaTestEnable;
m_Device->GetRenderState(D3DRS_ALPHATESTENABLE, &oldAlphaTestEnable);
DWORD oldSeparateAlphaBlendEnable;
m_Device->GetRenderState(D3DRS_SEPARATEALPHABLENDENABLE, &oldSeparateAlphaBlendEnable);
DWORD oldAlphaBlendEnable;
m_Device->GetRenderState(D3DRS_ALPHABLENDENABLE, &oldAlphaBlendEnable);
DWORD oldBlendOp;
m_Device->GetRenderState(D3DRS_BLENDOP, &oldBlendOp);
DWORD oldSrcBlend;
m_Device->GetRenderState(D3DRS_SRCBLEND, &oldSrcBlend);
DWORD oldDestBlend;
m_Device->GetRenderState(D3DRS_DESTBLEND, &oldDestBlend);
DWORD oldCullMode;
m_Device->GetRenderState(D3DRS_CULLMODE, &oldCullMode);
// Draw:
{
//Vector3 vvForward, vvUp, vvRight, vvPos;
double curTime = g_Hook_VClient_RenderView.GetCurTime();
bool inCampath = 1 <= g_Hook_VClient_RenderView.m_CamPath.GetSize()
&& g_Hook_VClient_RenderView.m_CamPath.GetLowerBound() <= curTime
&& curTime <= g_Hook_VClient_RenderView.m_CamPath.GetUpperBound();
bool campathCanEval = g_Hook_VClient_RenderView.m_CamPath.CanEval();
bool campathEnabled = g_Hook_VClient_RenderView.m_CamPath.Enabled_get();
bool cameraMightBeSelected = false;
m_Device->SetRenderState(D3DRS_SRGBWRITEENABLE, FALSE);
m_Device->SetRenderState(D3DRS_COLORWRITEENABLE, D3DCOLORWRITEENABLE_ALPHA|D3DCOLORWRITEENABLE_BLUE|D3DCOLORWRITEENABLE_GREEN|D3DCOLORWRITEENABLE_RED);
m_Device->SetRenderState(D3DRS_ZENABLE, D3DZB_TRUE);
m_Device->SetRenderState(D3DRS_ZWRITEENABLE, FALSE);
m_Device->SetRenderState(D3DRS_ZFUNC, D3DCMP_LESSEQUAL);
m_Device->SetRenderState(D3DRS_ALPHATESTENABLE, FALSE);
m_Device->SetRenderState(D3DRS_SEPARATEALPHABLENDENABLE, FALSE);
m_Device->SetRenderState(D3DRS_ALPHABLENDENABLE, TRUE);
m_Device->SetRenderState(D3DRS_BLENDOP, D3DBLENDOP_ADD);
m_Device->SetRenderState(D3DRS_SRCBLEND, D3DBLEND_SRCALPHA);
m_Device->SetRenderState(D3DRS_DESTBLEND, D3DBLEND_INVSRCALPHA);
m_Device->SetRenderState(D3DRS_CULLMODE, D3DCULL_CCW);
m_Device->SetVertexShader(vertexShader);
m_WorldToScreenMatrix = g_VEngineClient->WorldToScreenMatrix();
m_Device->SetVertexShaderConstantF(8, m_WorldToScreenMatrix.m[0], 4);
// Provide view plane info for line clipping:
{
double plane0[4]={0,0,0,1};
double planeN[4]={1,0,0,1};
//double planeR[4]={0,-1,0,1};
//double planeU[4]={0,0,1,1};
unsigned char P[4];
unsigned char Q[4];
double L[4][4];
double U[4][4];
double M[4][4] = {
m_WorldToScreenMatrix.m[0][0], m_WorldToScreenMatrix.m[0][1], m_WorldToScreenMatrix.m[0][2], 0,
m_WorldToScreenMatrix.m[1][0], m_WorldToScreenMatrix.m[1][1], m_WorldToScreenMatrix.m[1][2], 0,
m_WorldToScreenMatrix.m[2][0], m_WorldToScreenMatrix.m[2][1], m_WorldToScreenMatrix.m[2][2], 0,
m_WorldToScreenMatrix.m[3][0], m_WorldToScreenMatrix.m[3][1], m_WorldToScreenMatrix.m[3][2], -1,
};
double b0[4] = {
0 -m_WorldToScreenMatrix.m[0][3],
0 -m_WorldToScreenMatrix.m[1][3],
0 -m_WorldToScreenMatrix.m[2][3],
-m_WorldToScreenMatrix.m[3][3],
};
double bN[4] = {
0 -m_WorldToScreenMatrix.m[0][3],
0 -m_WorldToScreenMatrix.m[1][3],
1 -m_WorldToScreenMatrix.m[2][3],
-m_WorldToScreenMatrix.m[3][3],
};
/*
double bR[4] = {
1 -m_WorldToScreenMatrix.m[0][3],
0 -m_WorldToScreenMatrix.m[1][3],
0 -m_WorldToScreenMatrix.m[2][3],
-m_WorldToScreenMatrix.m[3][3],
};
double bU[4] = {
0 -m_WorldToScreenMatrix.m[0][3],
1 -m_WorldToScreenMatrix.m[1][3],
0 -m_WorldToScreenMatrix.m[2][3],
-m_WorldToScreenMatrix.m[3][3],
};
*/
if(!LUdecomposition(M, P, Q, L, U))
{
Tier0_Warning("AFXERROR in CCampathDrawer::OnPostRenderAllTools: LUdecomposition failed\n");
}
else
{
SolveWithLU(L, U, P, Q, b0, plane0);
SolveWithLU(L, U, P, Q, bN, planeN);
//SolveWithLU(L, U, P, Q, bR, planeR);
//SolveWithLU(L, U, P, Q, bU, planeU);
}
/*
vvPos = Vector3(plane0[0], plane0[1], plane0[2]);
vvForward = Vector3(planeN[0] -vvPos.X, planeN[1] -vvPos.Y, planeN[2]-vvPos.Z);
vvForward.Normalize();
vvRight = Vector3(planeR[0] -vvPos.X, planeR[1] -vvPos.Y, planeR[2]-vvPos.Z);
vvRight.Normalize();
vvUp = Vector3(planeU[0] -vvPos.X, planeU[1] -vvPos.Y, planeU[2]-vvPos.Z);
vvUp.Normalize();
*/
/*
Tier0_Msg("CCampathDrawer::OnPostRenderAllTools: curTime = %f\n",curTime);
Tier0_Msg("M[0]=%f %f %f %f\nM[1]=%f %f %f %f\nM[2]=%f %f %f %f\nM[3]=%f %f %f %f\n", M[0][0],M[0][1],M[0][2],M[0][3], M[1][0],M[1][1],M[1][2],M[1][3], M[2][0],M[2][1],M[2][2],M[2][3], M[3][0],M[3][1],M[3][2],M[3][3]);
Tier0_Msg("b0[0]=%f %f %f %f\n", b0[0], b0[1], b0[2], b0[3]);
Tier0_Msg("bN[0]=%f %f %f %f\n", bN[0], bN[1], bN[2], bN[3]);
Tier0_Msg("plane0=%f %f %f %f\n", plane0[0], plane0[1], plane0[2], plane0[3]);
Tier0_Msg("planeN=%f %f %f %f\n", planeN[0], planeN[1], planeN[2], planeN[3]);
*/
FLOAT vPlane0[4] = {(float)plane0[0], (float)plane0[1], (float)plane0[2], 0.0f};
Vector3 planeNormal(planeN[0] -plane0[0], planeN[1] -plane0[1], planeN[2] -plane0[2]);
planeNormal.Normalize();
FLOAT vPlaneN[4] = {(float)planeNormal.X, (float)planeNormal.Y, (float)planeNormal.Z, 0.0f};
m_Device->SetVertexShaderConstantF(49, vPlane0, 1);
m_Device->SetVertexShaderConstantF(50, vPlaneN, 1);
}
m_Device->SetPixelShader(pixelShader);
m_Device->SetFVF(CCampathDrawer_VertexFVF);
int screenWidth, screenHeight;
g_VEngineClient->GetScreenSize(screenWidth, screenHeight);
FLOAT newCScreenInfo[4] = { 0 != screenWidth ? 1.0f / screenWidth : 0.0f, 0 != screenHeight ? 1.0f / screenHeight : 0.0f, 0.0, 0.0f};
// Draw trajectory:
if(2 <= g_Hook_VClient_RenderView.m_CamPath.GetSize() && campathCanEval)
{
if(m_RebuildDrawing)
{
// Rebuild trajectory points.
// This operation can be quite expensive (up to O(N^2)),
// so it should be done only when s.th.
// changed (which is what we do here).
m_TrajectoryPoints.clear();
CamPathIterator last = g_Hook_VClient_RenderView.m_CamPath.GetBegin();
CamPathIterator it = last;
TempPoint * pts = new TempPoint[c_CameraTrajectoryMaxPointsPerInterval];
for(++it; it != g_Hook_VClient_RenderView.m_CamPath.GetEnd(); ++it)
{
double delta = it.GetTime() -last.GetTime();
for(size_t i = 0; i<c_CameraTrajectoryMaxPointsPerInterval; i++)
{
double t = last.GetTime() + delta*((double)i/(c_CameraTrajectoryMaxPointsPerInterval-1));
CamPathValue cpv = g_Hook_VClient_RenderView.m_CamPath.Eval(t);
pts[i].t = t;
pts[i].y = Vector3(cpv.X, cpv.Y, cpv.Z);
pts[i].nextPt = i+1 <c_CameraTrajectoryMaxPointsPerInterval ? &(pts[i+1]) : 0;
}
RamerDouglasPeucker(&(pts[0]), &(pts[c_CameraTrajectoryMaxPointsPerInterval-1]), c_CameraTrajectoryEpsilon);
// add all points except the last one (to avoid duplicates):
for(TempPoint * pt = &(pts[0]); pt && pt->nextPt; pt = pt->nextPt)
{
m_TrajectoryPoints.push_back(pt->t);
}
last = it;
}
// add last point:
m_TrajectoryPoints.push_back(pts[c_CameraTrajectoryMaxPointsPerInterval-1].t);
delete pts;
m_RebuildDrawing = false;
}
newCScreenInfo[2] = c_CameraTrajectoryPixelWidth;
m_Device->SetVertexShaderConstantF(48, newCScreenInfo, 1);
AutoPolyLineStart();
std::list<double>::iterator itPts = m_TrajectoryPoints.begin();
CamPathIterator itKeysLast = g_Hook_VClient_RenderView.m_CamPath.GetBegin();
CamPathIterator itKeysNext = itKeysLast;
++itKeysNext;
bool hasLastPt = false;
bool hasNextPt = false;
bool hasCurPt = false;
double lastPtTime;
CamPathValue lastPtValue;
double curPtTime;
CamPathValue curPtValue;
double nextPtTime;
CamPathValue nextPtValue;
do
{
if(hasNextPt)
{
hasLastPt = true;
lastPtTime = curPtTime;
lastPtValue = curPtValue;
hasCurPt = true;
curPtTime = nextPtTime;
curPtValue = nextPtValue;
hasNextPt = false;
}
else
{
hasCurPt = true;
curPtTime = *itPts;
curPtValue = g_Hook_VClient_RenderView.m_CamPath.Eval(curPtTime);
++itPts;
}
while(itKeysNext.GetTime() < curPtTime)
{
itKeysLast = itKeysNext;
++itKeysNext;
}
if(itPts != m_TrajectoryPoints.end())
{
hasNextPt = true;
nextPtTime = *itPts;
nextPtValue = g_Hook_VClient_RenderView.m_CamPath.Eval(nextPtTime);
++itPts;
}
else
{
// current point is last point.
hasNextPt = false;
nextPtValue = curPtValue;
}
if(!hasLastPt)
{
// current point is first point.
lastPtValue = curPtValue;
}
// emit current point:
{
double deltaTime = abs(curTime -curPtTime);
DWORD colour;
// determine colour:
if(deltaTime < 1.0)
{
double t = (deltaTime -0.0)/1.0;
colour = D3DCOLOR_RGBA(
ValToUCCondInv(255.0*t, curPtValue.Selected),
ValToUCCondInv(255, curPtValue.Selected),
ValToUCCondInv(0, curPtValue.Selected),
(unsigned char)(127*(1.0-t))+128
);
}
else
if(deltaTime < 2.0)
{
double t = (deltaTime -1.0)/1.0;
colour = D3DCOLOR_RGBA(
ValToUCCondInv(255, curPtValue.Selected),
ValToUCCondInv(255.0*(1.0-t), curPtValue.Selected),
ValToUCCondInv(0, curPtValue.Selected),
(unsigned char)(64*(1.0-t))+64
);
}
else
{
colour = D3DCOLOR_RGBA(
ValToUCCondInv(255, curPtValue.Selected),
ValToUCCondInv(0, curPtValue.Selected),
ValToUCCondInv(0, curPtValue.Selected),
64
);
}
AutoPolyLinePoint(
Vector3(lastPtValue.X,lastPtValue.Y,lastPtValue.Z)
, Vector3(curPtValue.X,curPtValue.Y,curPtValue.Z)
, colour
, Vector3(nextPtValue.X,nextPtValue.Y,nextPtValue.Z));
}
}
while(hasNextPt);
AutoPolyLineFlush();
}
// Draw keyframes:
{
newCScreenInfo[2] = c_CampathCrossPixelWidth;
m_Device->SetVertexShaderConstantF(48, newCScreenInfo, 1);
bool lpSelected = false;
double lpTime;
/*if(0 < g_Hook_VClient_RenderView.m_CamPath.GetSize())
{
// Test for not too unlikely hard case:
CamPathValue cpv = g_Hook_VClient_RenderView.m_CamPath.GetBegin().GetValue();
Vector3 current(cpv.X+76, cpv.Y+76, cpv.Z+76);
Vector3 previous(current.X+76, current.Y-1*4, current.Z);
Vector3 next(current.X+76, current.Y+1*4, current.Z);
Vector3 next2(current.X, current.Y+2*4, current.Z);
Vector3 next3(current.X+76, current.Y+3*4, current.Z);
Vector3 next4(current.X, current.Y+4*4, current.Z);
Vector3 next5(current.X+76, current.Y+5*4, current.Z);
AutoPolyLineStart();
AutoPolyLinePoint(previous, previous, D3DCOLOR_RGBA(255,0,0,255), current);
AutoPolyLinePoint(previous, current, D3DCOLOR_RGBA(255,0,0,255), next);
AutoPolyLinePoint(current, next, D3DCOLOR_RGBA(255,0,0,255), next2);
AutoPolyLinePoint(next, next2, D3DCOLOR_RGBA(255,0,0,255), next3);
AutoPolyLinePoint(next2, next3, D3DCOLOR_RGBA(255,0,0,255), next4);
AutoPolyLinePoint(next3, next4, D3DCOLOR_RGBA(255,0,0,255), next5);
AutoPolyLinePoint(next4, next5, D3DCOLOR_RGBA(255,0,0,255), next5);
AutoPolyLineFlush();
}*/
/*if(0 < g_Hook_VClient_RenderView.m_CamPath.GetSize())
{
CamPathValue cpv = g_Hook_VClient_RenderView.m_CamPath.GetBegin().GetValue();
float x = cpv.X * m_WorldToScreenMatrix.m[0][0] + cpv.Y * m_WorldToScreenMatrix.m[0][1] + cpv.Z * m_WorldToScreenMatrix.m[0][2] +m_WorldToScreenMatrix.m[0][3];
float y = cpv.X * m_WorldToScreenMatrix.m[1][0] + cpv.Y * m_WorldToScreenMatrix.m[1][1] + cpv.Z * m_WorldToScreenMatrix.m[1][2] +m_WorldToScreenMatrix.m[1][3];
float z = cpv.X * m_WorldToScreenMatrix.m[2][0] + cpv.Y * m_WorldToScreenMatrix.m[2][1] + cpv.Z * m_WorldToScreenMatrix.m[2][2] +m_WorldToScreenMatrix.m[2][3];
float w = cpv.X * m_WorldToScreenMatrix.m[3][0] + cpv.Y * m_WorldToScreenMatrix.m[3][1] + cpv.Z * m_WorldToScreenMatrix.m[3][2] +m_WorldToScreenMatrix.m[3][3];
float iw = w ? 1/w : 0;
Tier0_Msg("pt: %f %f %f %f -> %f %f %f %f\n",x,y,z,w,x*iw,y*iw,z*iw,w*iw);
}*/
for(CamPathIterator it = g_Hook_VClient_RenderView.m_CamPath.GetBegin(); it != g_Hook_VClient_RenderView.m_CamPath.GetEnd(); ++it)
{
double cpT = it.GetTime();
CamPathValue cpv = it.GetValue();
cameraMightBeSelected = cameraMightBeSelected || lpSelected && cpv.Selected && lpTime <= curTime && curTime <= cpT;
lpSelected = cpv.Selected;
lpTime = cpT;
double deltaTime = abs(curTime -cpT);
bool selected = cpv.Selected;
DWORD colour;
// determine colour:
if(deltaTime < 1.0)
{
double t = (deltaTime -0.0)/1.0;
colour = D3DCOLOR_RGBA(
ValToUCCondInv(255.0*t, selected),
ValToUCCondInv(255, selected),
ValToUCCondInv(0, selected),
(unsigned char)(127*(1.0-t))+128
);
}
else
if(deltaTime < 2.0)
{
double t = (deltaTime -1.0)/1.0;
colour = D3DCOLOR_RGBA(
ValToUCCondInv(255, selected),
ValToUCCondInv(255.0*(1.0-t), selected),
ValToUCCondInv(0, selected),
(unsigned char)(64*(1.0-t))+64
);
}
else
{
colour = D3DCOLOR_RGBA(
ValToUCCondInv(255, selected),
ValToUCCondInv(0, selected),
ValToUCCondInv(0, selected),
64
);
}
// x / forward line:
AutoSingleLine(
Vector3(cpv.X -c_CampathCrossRadius, cpv.Y, cpv.Z),
colour,
Vector3(cpv.X +c_CampathCrossRadius, cpv.Y, cpv.Z),
colour
);
// y / left line:
AutoSingleLine(
Vector3(cpv.X, cpv.Y -c_CampathCrossRadius, cpv.Z),
colour,
Vector3(cpv.X, cpv.Y +c_CampathCrossRadius, cpv.Z),
colour
);
// z / up line:
AutoSingleLine(
Vector3(cpv.X, cpv.Y, cpv.Z -c_CampathCrossRadius),
colour,
Vector3(cpv.X, cpv.Y, cpv.Z +c_CampathCrossRadius),
colour
);
}
AutoSingleLineFlush();
}
// Draw wireframe camera:
if(inCampath && campathCanEval)
{
newCScreenInfo[2] = c_CameraPixelWidth;
m_Device->SetVertexShaderConstantF(48, newCScreenInfo, 1);
DWORD colourCam = campathEnabled
? D3DCOLOR_RGBA(
ValToUCCondInv(255,cameraMightBeSelected),
ValToUCCondInv(0,cameraMightBeSelected),
ValToUCCondInv(255,cameraMightBeSelected),
128)
: D3DCOLOR_RGBA(
ValToUCCondInv(255,cameraMightBeSelected),
ValToUCCondInv(255,cameraMightBeSelected),
ValToUCCondInv(255,cameraMightBeSelected),
128);
DWORD colourCamUp = campathEnabled
? D3DCOLOR_RGBA(
ValToUCCondInv(0,cameraMightBeSelected),
ValToUCCondInv(255,cameraMightBeSelected),
ValToUCCondInv(0,cameraMightBeSelected),
128)
: D3DCOLOR_RGBA(
ValToUCCondInv(0,cameraMightBeSelected),
ValToUCCondInv(0,cameraMightBeSelected),
ValToUCCondInv(0,cameraMightBeSelected),
128);
CamPathValue cpv = g_Hook_VClient_RenderView.m_CamPath.Eval(curTime);
// limit to values as RenderView hook:
cpv.Fov = max(1,cpv.Fov);
cpv.Fov = min(179,cpv.Fov);
double forward[3], right[3], up[3];
QEulerAngles ang = cpv.R.ToQREulerAngles().ToQEulerAngles();
MakeVectors(ang.Roll, ang.Pitch, ang.Yaw, forward, right, up);
Vector3 vCp(cpv.X, cpv.Y, cpv.Z);
Vector3 vForward(forward);
Vector3 vUp(up);
Vector3 vRight(right);
//Tier0_Msg("----------------",curTime);
//Tier0_Msg("currenTime = %f",curTime);
//Tier0_Msg("vCp = %f %f %f\n", vCp.X, vCp.Y, vCp.Z);
double a = sin(cpv.Fov * M_PI / 360.0) * c_CameraRadius;
double b = a;
int screenWidth, screenHeight;
g_VEngineClient->GetScreenSize(screenWidth, screenHeight);
double aspectRatio = screenWidth ? (double)screenHeight / (double)screenWidth : 1.0;
b *= aspectRatio;
Vector3 vLU = vCp +(double)c_CameraRadius * vForward -a * vRight +b * vUp;
Vector3 vRU = vCp +(double)c_CameraRadius * vForward +a * vRight +b * vUp;
Vector3 vLD = vCp +(double)c_CameraRadius * vForward -a * vRight -b * vUp;
Vector3 vRD = vCp +(double)c_CameraRadius * vForward +a * vRight -b * vUp;
Vector3 vMU = vLU +(vRU -vLU)/2;
Vector3 vMUU = vMU +(double)c_CameraRadius * vUp;
AutoSingleLine(vCp, colourCam, vLD, colourCam);
AutoSingleLine(vCp, colourCam, vRD, colourCam);
AutoSingleLine(vCp, colourCam, vLU, colourCam);
AutoSingleLine(vCp, colourCam, vRU, colourCam);
AutoSingleLine(vLD, colourCam, vRD, colourCam);
AutoSingleLine(vRD, colourCam, vRU, colourCam);
AutoSingleLine(vRU, colourCam, vLU, colourCam);
AutoSingleLine(vLU, colourCam, vLD, colourCam);
AutoSingleLine(vMU, colourCam, vMUU, colourCamUp);
AutoSingleLineFlush();
//
/*
colourCam = D3DCOLOR_RGBA(255, 0, 0, 255);
colourCamUp = D3DCOLOR_RGBA(255, 255, 0, 255);
vCp = vvPos;
vForward = vvForward;
vUp = vvUp;
vRight = vvRight;
//Tier0_Msg("vCp2 = %f %f %f\n", vCp.X, vCp.Y, vCp.Z);
vLU = vCp +(double)c_CameraRadius * vForward -a * vRight +b * vUp;
vRU = vCp +(double)c_CameraRadius * vForward +a * vRight +b * vUp;
vLD = vCp +(double)c_CameraRadius * vForward -a * vRight -b * vUp;
vRD = vCp +(double)c_CameraRadius * vForward +a * vRight -b * vUp;
vMU = vLU +(vRU -vLU)/2;
vMUU = vMU +(double)c_CameraRadius * vUp;
AutoSingleLine(vCp, colourCam, vLD, colourCam);
AutoSingleLine(vCp, colourCam, vRD, colourCam);
AutoSingleLine(vCp, colourCam, vLU, colourCam);
AutoSingleLine(vCp, colourCam, vRU, colourCam);
AutoSingleLine(vLD, colourCam, vRD, colourCam);
AutoSingleLine(vRD, colourCam, vRU, colourCam);
AutoSingleLine(vRU, colourCam, vLU, colourCam);
AutoSingleLine(vLU, colourCam, vLD, colourCam);
AutoSingleLine(vMU, colourCam, vMUU, colourCamUp);
AutoSingleLineFlush();
*/
}
}
// Restore device state:
m_Device->SetPixelShader(oldPixelShader);
if(oldPixelShader) oldPixelShader->Release();
m_Device->SetVertexShader(oldVertexShader);
if(oldVertexShader) oldVertexShader->Release();
m_Device->SetStreamSource(0, oldVertexBuffer, oldVertexBufferOffset, oldVertexBufferStride);
if(oldVertexBuffer) oldVertexBuffer->Release();
m_Device->SetIndices(oldIndexBuffer);
if(oldIndexBuffer) oldIndexBuffer->Release();
m_Device->SetFVF(oldFVF);
m_Device->SetVertexDeclaration(oldDeclaration);
if(oldDeclaration) oldDeclaration->Release();
m_Device->SetVertexShaderConstantF(8, oldCViewProj[0], 4);
m_Device->SetVertexShaderConstantF(48, oldCScreenInfo, 1);
m_Device->SetVertexShaderConstantF(49, oldCPlane0, 1);
m_Device->SetVertexShaderConstantF(50, oldCPlaneN, 1);
m_Device->SetRenderState(D3DRS_CULLMODE, oldCullMode);
m_Device->SetRenderState(D3DRS_DESTBLEND, oldDestBlend);
m_Device->SetRenderState(D3DRS_SRCBLEND, oldSrcBlend);
m_Device->SetRenderState(D3DRS_BLENDOP, oldBlendOp);
m_Device->SetRenderState(D3DRS_ALPHABLENDENABLE, oldAlphaBlendEnable);
m_Device->SetRenderState(D3DRS_SEPARATEALPHABLENDENABLE, oldSeparateAlphaBlendEnable);
m_Device->SetRenderState(D3DRS_ALPHATESTENABLE, oldAlphaTestEnable);
m_Device->SetRenderState(D3DRS_ZFUNC, oldZFunc);
m_Device->SetRenderState(D3DRS_ZWRITEENABLE, oldZWriteEnable);
m_Device->SetRenderState(D3DRS_ZENABLE, oldZEnable);
m_Device->SetRenderState(D3DRS_COLORWRITEENABLE, oldColorWriteEnable);
m_Device->SetRenderState(D3DRS_SRGBWRITEENABLE, oldSrgbWriteEnable);
}
//--------------------------------------------------------------
void ofApp::setup(){
ofSetLogLevel(OF_LOG_VERBOSE);
glEnable(GL_DEPTH_TEST);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> clouds;
mesh.load(ofToDataPath("out.ply"));
cloud = ofxPCL::toPCL<ofxPCL::PointXYZCloud>(mesh);
textures.resize(2);
textures.at(0).loadImage(ofToDataPath("tex0.jpg"));
textures.at(1).loadImage(ofToDataPath("tex1.jpg"));
pcl::SacModel model_type = pcl::SACMODEL_PLANE;
float distance_threshold = 30;
int min_points_limit = 10;
int max_segment_count = 30;
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients());
pcl::PointIndices::Ptr inliers(new pcl::PointIndices());
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(false);
seg.setModelType(model_type);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setDistanceThreshold(distance_threshold);
seg.setMaxIterations(500);
pcl::PointCloud<pcl::PointXYZ>::Ptr temp(new pcl::PointCloud<pcl::PointXYZ>(*cloud));
const size_t original_szie = temp->points.size();
pcl::ExtractIndices<pcl::PointXYZ> extract;
int segment_count = 0;
while (temp->size() > original_szie * 0.3)
{
if (segment_count > max_segment_count) break;
segment_count++;
seg.setInputCloud(temp);
seg.segment(*inliers, *coefficients);
if (inliers->indices.size() < min_points_limit)
break;
pcl::PointCloud<pcl::PointXYZ>::Ptr filterd_point_cloud(new pcl::PointCloud<pcl::PointXYZ>);
extract.setInputCloud(temp);
extract.setIndices(inliers);
extract.setNegative(false);
extract.filter(*filterd_point_cloud);
if (filterd_point_cloud->points.size() > 0)
{
clouds.push_back(filterd_point_cloud);
}
extract.setNegative(true);
extract.filter(*temp);
ofMesh m;
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_with_normals(new pcl::PointCloud<pcl::PointNormal>);
ofxPCL::normalEstimation(filterd_point_cloud, cloud_with_normals);
m = ofxPCL::triangulate(cloud_with_normals, 100);
m.clearColors();
jointMesh.addVertices(m.getVertices());
jointMesh.addColors(m.getColors());
ofVec3f center = m.getCentroid();
ofVec3f planeNormal(coefficients->values[0], coefficients->values[1], coefficients->values[2]);
ofVec3f tangent, bitangent;
ofVec3f arb(0, 1, 0);
tangent = arb.cross(planeNormal).normalize();
bitangent = planeNormal.cross(tangent).normalize();
for(int j = 0; j < m.getNumVertices(); j++) {
float x = m.getVertex(j).dot(tangent) * 0.001;
x = x - (long)x;
if(x < 0) x += 1;
float y = m.getVertex(j).dot(bitangent) * 0.001;
y = y - (long)y;
if(y < 0) y += 1;
m.addTexCoord(ofVec2f(x, y));
}
meshes.push_back(m);
}
ofxPCL::convert(temp, meshResidual);
ofLogVerbose() << clouds.size() << " meshes extracted";
center = jointMesh.getCentroid();
}
Intersection* Plane3D::testRayIntersection(Ray r)
{
/*std::cout << "ray start pos " << r.startPosition.x << " " << r.startPosition.y << " " << r.startPosition.z << std::endl;
std::cout << "ray direction " << r.direction.x << " " << r.direction.y << " " << r.direction.z << std::endl;*/
//The ray is described by Point on ray = start(O) + direction(D) * t
//If a point B on the ray hits a surface it must be inside the plane of that surface
//We can check if B is inside the surface by making a vector between a point on the surface(A) and B
//and dot multiply it with the plan's normal and see if it becomes zero
//(A-B)*n = 0
//Then the point B is on the plane
//We want to know at what t-value we get a point B on the ray that is inside the plane
//After some calculations this formula was found
//t = (O - A) dot n / D dot n
//translate the plane to it's local coordinate system rotate it and tranlate it back
glm::mat4 translation = glm::translate(glm::mat4(1.f), -position);
glm::mat4 translationBack = glm::translate(glm::mat4(1.f), position);
glm::mat4 rotat = glm::rotate(glm::rotate(glm::rotate(glm::mat4(1.f), -rotation.x, glm::vec3(1, 0, 0)), -rotation.y, glm::vec3(0, 1, 0)), -rotation.z, glm::vec3(0, 0, 1));
glm::mat4 toLocal = rotat * translation;
glm::vec4 temp(translationBack * toLocal * glm::vec4(position + glm::vec3(-dimensions.x / 2, -dimensions.y / 2, 0), 1));
glm::vec3 lowerLeftCorner(temp.x, temp.y, temp.z);
temp = glm::vec4(translationBack * toLocal * glm::vec4(position + glm::vec3(dimensions.x / 2, -dimensions.y / 2, 0), 1));
glm::vec3 lowerRightCorner(temp.x, temp.y, temp.z);
temp = glm::vec4(translationBack * toLocal * glm::vec4(position + glm::vec3(-dimensions.x / 2, dimensions.y / 2, 0), 1));
glm::vec3 upperLeftCorner(temp.x, temp.y, temp.z);
//Find plane normal
glm::vec3 planeNormal(glm::normalize(glm::cross(lowerRightCorner - lowerLeftCorner, upperLeftCorner - lowerLeftCorner)));
/*std::cout << "lower left " << lowerLeftCorner.x << " " << lowerLeftCorner.y << " " << lowerLeftCorner.z << std::endl;
std::cout << "lower right " << lowerRightCorner.x << " " << lowerRightCorner.y << " " << lowerRightCorner.z << std::endl;
std::cout << "upper left " << upperLeftCorner.x << " " << upperLeftCorner.y << " " << upperLeftCorner.z << std::endl;
std::cout << "ray start pos " << r.startPosition.x << " " << r.startPosition.y << " " << r.startPosition.z << std::endl;
std::cout << "ray direction " << r.direction.x << " " << r.direction.y << " " << r.direction.z << std::endl;*/
//If D dot n == 0 the direction goes along the plane and the ray won't hit the plane
//If D dot n < 0 the surface can't be seen from the ray's starting point
//But if the ray is a shadow ray it doesn't matter if we can't see the object, just that it is in the way
float DdotN = glm::dot(-r.direction, planeNormal);
if (r.shadowRay)
{
if (DdotN < EPSILON && DdotN > -EPSILON)
return nullptr;
}
else
if (DdotN < EPSILON){
//std::cout << "DdotN <= 0 " << std::endl;
return nullptr;
}
//std::cout << "ray direction " << r.direction.x << " " << r.direction.y << " " << r.direction.z << std::endl;
//std::cout << "planeNormal " << planeNormal.x << " " << planeNormal.y << " " << planeNormal.z << std::endl;
//Find point inside plane -> position, which is the center of the plane
//Find intersection
float t;
if (r.startPosition - position != planeNormal)
{
t = glm::dot(r.startPosition - position, planeNormal) / DdotN;
//std::cout << "1t = " << t << std::endl;
}
else
{
glm::vec3 temp = r.startPosition - lowerLeftCorner;
//std::cout << "vector on plane? " << temp.x << " " << temp.y << " " << temp.z << std::endl;
t = glm::dot(r.startPosition - lowerLeftCorner, planeNormal) / DdotN;
//std::cout << "2t = " << t << " dot " << glm::dot(r.startPosition - lowerLeftCorner, planeNormal) << std::endl;
}
//If t < 0 the plane is behind or inside the ray origin and we aren't interested in it
if (t < EPSILON){
//std::cout << "t <= 0 " << std::endl;
return nullptr;
}
//Is B inside the surface bounds?
//Create vectors that decribe the bounds
//"Inside" is on the right side of the vector
temp = glm::vec4(translationBack * toLocal * glm::vec4(position + glm::vec3(dimensions.x / 2, dimensions.y / 2, 0), 1));
glm::vec3 upperRightCorner(temp.x, temp.y, temp.z);
//std::cout << "upper right " << upperRightCorner.x << " " << upperRightCorner.y << " " << upperRightCorner.z << std::endl;
glm::vec3 leftSide(upperLeftCorner - lowerLeftCorner);
glm::vec3 topSide(upperRightCorner - upperLeftCorner);
glm::vec3 rightSide(lowerRightCorner - upperRightCorner);
glm::vec3 bottomSide(lowerLeftCorner - lowerRightCorner);
//To know if B is on the correct side of the vector we take the cross product of
//the bounding vector and a vector from the same starting point as the bounding vector and ends in B
//If the result is a vector along the plane's normal, the point B is on the correct side of the bounding vector
//If the result is a vector against the plane's normal the point is on the wrong side
//To know if the vector is going along the normal we take the dot product between them
//If it is larger than zero they go with each other
//leftSide
glm::vec3 B(r.startPosition + t * r.direction);
glm::vec3 Bvector(B - lowerLeftCorner);
if (glm::dot(glm::cross(Bvector, leftSide), planeNormal) > 0)
{
//topSide
Bvector = glm::vec3(B - upperLeftCorner);
if (glm::dot(glm::cross(Bvector, topSide), planeNormal) > 0)
{
//rightSide
Bvector = glm::vec3(B - upperRightCorner);
if (glm::dot(glm::cross(Bvector, rightSide), planeNormal) > 0)
{
//bottomSide
Bvector = glm::vec3(B - lowerRightCorner);
if (glm::dot(glm::cross(Bvector, bottomSide), planeNormal) > 0)
{
// If the object is blocked
if (t > r.tMax){
//std::cout << "t > r.tMax" << std::endl;
return nullptr;
}
//else
r.tMax = t;
glm::vec3 intersectionPoint(r.startPosition + t * r.direction);
//std::cout << "intersected object" << std::endl;
return new Intersection(intersectionPoint, planeNormal, color, reflectionCoef, t);
}
}
}
}
//std::cout << "not inside bounds" << std::endl;
return nullptr;
}
void ColladaConverter::ConvertRigidBodyRef( btRigidBodyInput& rbInput,btRigidBodyOutput& rbOutput)
{
const domRigid_body::domTechnique_commonRef techniqueRef = rbInput.m_rigidBodyRef2->getTechnique_common();
if (techniqueRef)
{
if (techniqueRef->getMass())
{
rbOutput.m_mass = (float) techniqueRef->getMass()->getValue();
}
if (techniqueRef->getDynamic())
{
rbOutput.m_isDynamics = techniqueRef->getDynamic()->getValue();
}
//a hack to interpret <extra> PhysX profile:
//when <kinematic> is true, make <dynamic> false...
//using the DOM is a pain...
const domExtra_Array& extraArray = rbInput.m_rigidBodyRef2->getExtra_array();
unsigned int s=0;
for (s = 0;s< extraArray.getCount();s++)
{
const domExtraRef extraRef = extraArray[s];
const domTechnique_Array techniqueArray = extraRef->getTechnique_array();
unsigned int t=0;
for (t=0;t<techniqueArray.getCount();t++)
{
const domTechniqueRef techRef = techniqueArray[t];
const daeElementRefArray elemRefArray = techRef->getContents();
unsigned int u = 0;
for (u=0;u<elemRefArray.getCount();u++)
{
daeElementRef elemRef = elemRefArray[u];
daeString elemName = elemRef->getElementName();
if (elemName && !strcmp(elemName,"kinematic"))
{
//how can I make this cast safe?
const domAny* myAny = (const domAny*)elemRef.cast();
daeString myVal = myAny->getValue();
if (myVal)
{
if (!strcmp(myVal,"true"))
{
printf("revert bug in PhysX .dae export -> <kinematic>true</kinematic> means <dynamic>false</dynamic>\n");
rbOutput.m_isDynamics = false;
}
}
}
}
}
}
//shapes
for (s=0;s<techniqueRef->getShape_array().getCount();s++)
{
domRigid_body::domTechnique_common::domShapeRef shapeRef = techniqueRef->getShape_array()[s];
if (shapeRef->getPlane())
{
domPlaneRef planeRef = shapeRef->getPlane();
if (planeRef->getEquation())
{
const domFloat4 planeEq = planeRef->getEquation()->getValue();
btVector3 planeNormal((btScalar)planeEq.get(0),(btScalar)planeEq.get(1),(btScalar)planeEq.get(2));
btScalar planeConstant = (btScalar)planeEq.get(3)*(btScalar)m_unitMeterScaling;
rbOutput.m_colShape = new btStaticPlaneShape(planeNormal,planeConstant);
}
}
if (shapeRef->getBox())
{
domBoxRef boxRef = shapeRef->getBox();
domBox::domHalf_extentsRef domHalfExtentsRef = boxRef->getHalf_extents();
domFloat3& halfExtents = domHalfExtentsRef->getValue();
btScalar x = (btScalar)halfExtents.get(0)*m_unitMeterScaling;
btScalar y = (btScalar)halfExtents.get(1)*m_unitMeterScaling;
btScalar z = (btScalar)halfExtents.get(2)*m_unitMeterScaling;
rbOutput.m_colShape = new btBoxShape(btVector3(x,y,z));
}
if (shapeRef->getSphere())
{
domSphereRef sphereRef = shapeRef->getSphere();
domSphere::domRadiusRef radiusRef = sphereRef->getRadius();
btScalar radius = (btScalar)radiusRef->getValue()*m_unitMeterScaling;
rbOutput.m_colShape = new btSphereShape(radius);
}
if (shapeRef->getCylinder())
{
domCylinderRef cylinderRef = shapeRef->getCylinder();
domFloat height = cylinderRef->getHeight()->getValue()*m_unitMeterScaling;
domFloat2 radius2 = cylinderRef->getRadius()->getValue();
domFloat radius0 = radius2.get(0)*m_unitMeterScaling;
//Cylinder around the local Y axis
rbOutput.m_colShape = new btCylinderShapeZ(btVector3((btScalar)radius0,(btScalar)height,(btScalar)radius0));
}
if (shapeRef->getInstance_geometry())
{
const domInstance_geometryRef geomInstRef = shapeRef->getInstance_geometry();
daeElement* geomElem = geomInstRef->getUrl().getElement();
//elemRef->getTypeName();
domGeometry* geom = (domGeometry*) geomElem;
if (geom && geom->getMesh())
{
const domMeshRef meshRef = geom->getMesh();
//it can be either triangle mesh, or we just pick the vertices/positions
if (meshRef->getTriangles_array().getCount())
{
btTriangleMesh* trimesh = new btTriangleMesh();
for (unsigned int tg = 0;tg<meshRef->getTriangles_array().getCount();tg++)
{
domTrianglesRef triRef = meshRef->getTriangles_array()[tg];
const domPRef pRef = triRef->getP();
btIndexedMesh meshPart;
meshPart.m_triangleIndexStride=0;
int vertexoffset = -1;
domInputLocalOffsetRef indexOffsetRef;
for (unsigned int w=0;w<triRef->getInput_array().getCount();w++)
{
domUint offset = triRef->getInput_array()[w]->getOffset();
daeString str = triRef->getInput_array()[w]->getSemantic();
if (!strcmp(str,"VERTEX"))
{
indexOffsetRef = triRef->getInput_array()[w];
vertexoffset = (int) offset;
}
if ((int) offset > (int) meshPart.m_triangleIndexStride)
{
meshPart.m_triangleIndexStride = (int) offset;
}
}
meshPart.m_triangleIndexStride++;
domListOfUInts indexArray =triRef->getP()->getValue();
//int* m_triangleIndexBase;
meshPart.m_numTriangles = (int) triRef->getCount();
const domVerticesRef vertsRef = meshRef->getVertices();
size_t numInputs = vertsRef->getInput_array().getCount();
for (size_t i=0;i<numInputs;i++)
{
domInputLocalRef localRef = vertsRef->getInput_array()[i];
daeString str = localRef->getSemantic();
if ( !strcmp(str,"POSITION"))
{
domURIFragmentType& frag = localRef->getSource();
daeElementConstRef constElem = frag.getElement();
const domSourceRef node = *(const domSourceRef*)&constElem;
const domFloat_arrayRef flArray = node->getFloat_array();
if (flArray)
{
const domListOfFloats& listFloats = flArray->getValue();
int k=vertexoffset;
int t=0;
int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
for (;t<meshPart.m_numTriangles;t++)
{
btVector3 verts[3];
int index0;
for (int i=0;i<3;i++)
{
index0 = (int) indexArray.get(k)*vertexStride;
domFloat fl0 = listFloats.get(index0);
domFloat fl1 = listFloats.get(index0+1);
domFloat fl2 = listFloats.get(index0+2);
k+=meshPart.m_triangleIndexStride;
verts[i].setValue((btScalar)fl0*m_unitMeterScaling,(btScalar)fl1*m_unitMeterScaling,(btScalar)fl2*m_unitMeterScaling);
}
trimesh->addTriangle(verts[0],verts[1],verts[2]);
}
}
}
}
if (rbOutput.m_isDynamics)
{
printf("moving concave <mesh> not supported, transformed into convex\n");
rbOutput.m_colShape = new btConvexTriangleMeshShape(trimesh);
} else
{
printf("static concave triangle <mesh> added\n");
bool useQuantizedAabbCompression = false;
rbOutput.m_colShape = new btBvhTriangleMeshShape(trimesh,useQuantizedAabbCompression);
}
}
} else
{
btConvexHullShape* convexHull = new btConvexHullShape();
int numAddedVerts = 0;
const domVerticesRef vertsRef = meshRef->getVertices();
size_t numInputs = vertsRef->getInput_array().getCount();
for (size_t i=0;i<numInputs;i++)
{
domInputLocalRef localRef = vertsRef->getInput_array()[i];
daeString str = localRef->getSemantic();
if ( !strcmp(str,"POSITION"))
{
domURIFragmentType& frag = localRef->getSource();
daeElementConstRef constElem = frag.getElement();
const domSourceRef node = *(const domSourceRef*)&constElem;
const domFloat_arrayRef flArray = node->getFloat_array();
if (flArray)
{
const domListOfFloats& listFloats = flArray->getValue();
int vertexStride = 3;//instead of hardcoded stride, should use the 'accessor'
size_t vertIndex = 0;
for (vertIndex = 0;vertIndex < listFloats.getCount();vertIndex+=vertexStride)
{
domFloat fl0 = listFloats.get(vertIndex);
domFloat fl1 = listFloats.get(vertIndex+1);
domFloat fl2 = listFloats.get(vertIndex+2);
convexHull->addPoint(btPoint3((btScalar)fl0,(btScalar)fl1,(btScalar)fl2) * m_unitMeterScaling);
}
}
}
}
//convexHull->addPoint();
if (numAddedVerts > 0)
{
rbOutput.m_colShape = convexHull;
} else
{
delete convexHull;
printf("no vertices found for convex hull\n");
}
}
}
if (geom && geom->getConvex_mesh())
{
{
const domConvex_meshRef convexRef = geom->getConvex_mesh();
daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
if ( otherElemRef != NULL )
{
domGeometryRef linkedGeom = *(domGeometryRef*)&otherElemRef;
printf( "otherLinked\n");
} else
{
printf("convexMesh polyCount = %i\n",convexRef->getPolygons_array().getCount());
printf("convexMesh triCount = %i\n",convexRef->getTriangles_array().getCount());
}
}
btConvexHullShape* convexHullShape = new btConvexHullShape(0,0);
//it is quite a trick to get to the vertices, using Collada.
//we are not there yet...
const domConvex_meshRef convexRef = geom->getConvex_mesh();
//daeString urlref = convexRef->getConvex_hull_of().getURI();
daeString urlref2 = convexRef->getConvex_hull_of().getOriginalURI();
if (urlref2)
{
daeElementRef otherElemRef = convexRef->getConvex_hull_of().getElement();
// if ( otherElemRef != NULL )
// {
// domGeometryRef linkedGeom = *(domGeometryRef*)&otherElemRef;
// Load all the geometry libraries
for ( unsigned int i = 0; i < m_dom->getLibrary_geometries_array().getCount(); i++)
{
domLibrary_geometriesRef libgeom = m_dom->getLibrary_geometries_array()[i];
//int index = libgeom->findLastIndexOf(urlref2);
//can't find it
for ( unsigned int i = 0; i < libgeom->getGeometry_array().getCount(); i++)
{
//ReadGeometry( );
domGeometryRef lib = libgeom->getGeometry_array()[i];
if (!strcmp(lib->getId(),urlref2))
{
//found convex_hull geometry
domMesh *meshElement = lib->getMesh();//linkedGeom->getMesh();
if (meshElement)
{
const domVerticesRef vertsRef = meshElement->getVertices();
size_t numInputs = vertsRef->getInput_array().getCount();
for (size_t i=0;i<numInputs;i++)
{
domInputLocalRef localRef = vertsRef->getInput_array()[i];
daeString str = localRef->getSemantic();
if ( !strcmp(str,"POSITION"))
{
domURIFragmentType& frag = localRef->getSource();
daeElementConstRef constElem = frag.getElement();
const domSourceRef node = *(const domSourceRef*)&constElem;
const domFloat_arrayRef flArray = node->getFloat_array();
if (flArray)
{
int numElem = (int) flArray->getCount();
const domListOfFloats& listFloats = flArray->getValue();
for (int k=0;k+2<numElem;k+=3)
{
domFloat fl0 = listFloats.get(k);
domFloat fl1 = listFloats.get(k+1);
domFloat fl2 = listFloats.get(k+2);
//printf("float %f %f %f\n",fl0,fl1,fl2);
convexHullShape->addPoint(btPoint3((btScalar)fl0,(btScalar)fl1,(btScalar)fl2) * m_unitMeterScaling);
}
}
}
}
}
}
}
}
} else
{
//no getConvex_hull_of but direct vertices
const domVerticesRef vertsRef = convexRef->getVertices();
size_t numInputs = vertsRef->getInput_array().getCount();
for (size_t i=0;i<numInputs;i++)
{
domInputLocalRef localRef = vertsRef->getInput_array()[i];
daeString str = localRef->getSemantic();
if ( !strcmp(str,"POSITION"))
{
domURIFragmentType& frag = localRef->getSource();
daeElementConstRef constElem = frag.getElement();
const domSourceRef node = *(const domSourceRef*)&constElem;
const domFloat_arrayRef flArray = node->getFloat_array();
if (flArray)
{
int numElem = (int) flArray->getCount();
const domListOfFloats& listFloats = flArray->getValue();
for (int k=0;k+2<numElem;k+=3)
{
domFloat fl0 = listFloats.get(k);
domFloat fl1 = listFloats.get(k+1);
domFloat fl2 = listFloats.get(k+2);
//printf("float %f %f %f\n",fl0,fl1,fl2);
convexHullShape->addPoint(btPoint3((btScalar)fl0,(btScalar)fl1,(btScalar)fl2)*m_unitMeterScaling);
}
}
}
}
}
if (convexHullShape->getNumVertices())
{
rbOutput.m_colShape = convexHullShape;
printf("created convexHullShape with %i points\n",convexHullShape->getNumVertices());
} else
{
delete convexHullShape;
printf("failed to create convexHullShape\n");
}
//domGeometryRef linkedGeom = *(domGeometryRef*)&otherElemRef;
printf("convexmesh\n");
}
}
//if more then 1 shape, or a non-identity local shapetransform
//use a compound
bool hasShapeLocalTransform = ((shapeRef->getRotate_array().getCount() > 0) ||
(shapeRef->getTranslate_array().getCount() > 0));
if (rbOutput.m_colShape)
{
if ((techniqueRef->getShape_array().getCount()>1) ||
(hasShapeLocalTransform))
{
if (!rbOutput.m_compoundShape)
{
rbOutput.m_compoundShape = new btCompoundShape();
}
btTransform localTransform;
localTransform.setIdentity();
if (hasShapeLocalTransform)
{
localTransform = GetbtTransformFromCOLLADA_DOM(
emptyMatrixArray,
shapeRef->getRotate_array(),
shapeRef->getTranslate_array(),
m_unitMeterScaling
);
}
rbOutput.m_compoundShape->addChildShape(localTransform,rbOutput.m_colShape);
rbOutput.m_colShape = 0;
}
}
}//for each shape
}
}
bool btPolyhedralConvexShape::initializePolyhedralFeatures()
{
if (m_polyhedron)
btAlignedFree(m_polyhedron);
void* mem = btAlignedAlloc(sizeof(btConvexPolyhedron),16);
m_polyhedron = new (mem) btConvexPolyhedron;
btAlignedObjectArray<btVector3> tmpVertices;
for (int i=0;i<getNumVertices();i++)
{
btVector3& newVertex = tmpVertices.expand();
getVertex(i,newVertex);
}
btConvexHullComputer conv;
conv.compute(&tmpVertices[0].getX(), sizeof(btVector3),tmpVertices.size(),0.f,0.f);
btAlignedObjectArray<btVector3> faceNormals;
int numFaces = conv.faces.size();
faceNormals.resize(numFaces);
btConvexHullComputer* convexUtil = &conv;
m_polyhedron->m_faces.resize(numFaces);
int numVertices = convexUtil->vertices.size();
m_polyhedron->m_vertices.resize(numVertices);
for (int p=0;p<numVertices;p++)
{
m_polyhedron->m_vertices[p] = convexUtil->vertices[p];
}
for (int i=0;i<numFaces;i++)
{
int face = convexUtil->faces[i];
//printf("face=%d\n",face);
const btConvexHullComputer::Edge* firstEdge = &convexUtil->edges[face];
const btConvexHullComputer::Edge* edge = firstEdge;
btVector3 edges[3];
int numEdges = 0;
//compute face normals
//btScalar maxCross2 = 0.f;
//int chosenEdge = -1;
do
{
int src = edge->getSourceVertex();
m_polyhedron->m_faces[i].m_indices.push_back(src);
int targ = edge->getTargetVertex();
btVector3 wa = convexUtil->vertices[src];
btVector3 wb = convexUtil->vertices[targ];
btVector3 newEdge = wb-wa;
newEdge.normalize();
if (numEdges<2)
edges[numEdges++] = newEdge;
edge = edge->getNextEdgeOfFace();
} while (edge!=firstEdge);
btScalar planeEq = 1e30f;
if (numEdges==2)
{
faceNormals[i] = edges[0].cross(edges[1]);
faceNormals[i].normalize();
m_polyhedron->m_faces[i].m_plane[0] = -faceNormals[i].getX();
m_polyhedron->m_faces[i].m_plane[1] = -faceNormals[i].getY();
m_polyhedron->m_faces[i].m_plane[2] = -faceNormals[i].getZ();
m_polyhedron->m_faces[i].m_plane[3] = planeEq;
}
else
{
btAssert(0);//degenerate?
faceNormals[i].setZero();
}
for (int v=0;v<m_polyhedron->m_faces[i].m_indices.size();v++)
{
btScalar eq = m_polyhedron->m_vertices[m_polyhedron->m_faces[i].m_indices[v]].dot(faceNormals[i]);
if (planeEq>eq)
{
planeEq=eq;
}
}
m_polyhedron->m_faces[i].m_plane[3] = planeEq;
}
if (m_polyhedron->m_faces.size() && conv.vertices.size())
{
for (int f=0;f<m_polyhedron->m_faces.size();f++)
{
btVector3 planeNormal(m_polyhedron->m_faces[f].m_plane[0],m_polyhedron->m_faces[f].m_plane[1],m_polyhedron->m_faces[f].m_plane[2]);
btScalar planeEq = m_polyhedron->m_faces[f].m_plane[3];
btVector3 supVec = localGetSupportingVertex(-planeNormal);
if (supVec.dot(planeNormal)<planeEq)
{
m_polyhedron->m_faces[f].m_plane[0] *= -1;
m_polyhedron->m_faces[f].m_plane[1] *= -1;
m_polyhedron->m_faces[f].m_plane[2] *= -1;
m_polyhedron->m_faces[f].m_plane[3] *= -1;
int numVerts = m_polyhedron->m_faces[f].m_indices.size();
for (int v=0;v<numVerts/2;v++)
{
btSwap(m_polyhedron->m_faces[f].m_indices[v],m_polyhedron->m_faces[f].m_indices[numVerts-1-v]);
}
}
}
}
m_polyhedron->initialize();
return true;
}
