const CCVector3 CCCameraBase::getDirection() { CCVector3 direction = lookAt; direction.sub( rotatedPosition ); CCVector3Normalize( direction ); return direction; }
void ccClipBox::update() { if (m_entityContainer.getChildrenNumber() == 0) { return; } //remove any existing clipping plane { for (unsigned ci = 0; ci < m_entityContainer.getChildrenNumber(); ++ci) { m_entityContainer.getChild(ci)->removeAllClipPlanes(); } } //now add the 6 box clipping planes ccBBox extents; ccGLMatrix transformation; get(extents, transformation); CCVector3 C = transformation * extents.getCenter(); CCVector3 halfDim = extents.getDiagVec() / 2; //for each dimension for (unsigned d = 0; d < 3; ++d) { CCVector3 N = transformation.getColumnAsVec3D(d); //positive side { ccClipPlane posPlane; posPlane.equation.x = N.x; posPlane.equation.y = N.y; posPlane.equation.z = N.z; //compute the 'constant' coefficient knowing that P belongs to the plane if (P - (C - half_dim * N)).N = 0 posPlane.equation.w = -static_cast<double>(C.dot(N)) + halfDim.u[d]; for (unsigned ci = 0; ci < m_entityContainer.getChildrenNumber(); ++ci) { m_entityContainer.getChild(ci)->addClipPlanes(posPlane); } } //negative side { ccClipPlane negPlane; negPlane.equation.x = -N.x; negPlane.equation.y = -N.y; negPlane.equation.z = -N.z; //compute the 'constant' coefficient knowing that P belongs to the plane if (P - (C + half_dim * N)).N = 0 //negPlane.equation.w = -(static_cast<double>(C.dot(N)) + halfDim.u[d]); negPlane.equation.w = static_cast<double>(C.dot(N)) + halfDim.u[d]; for (unsigned ci = 0; ci < m_entityContainer.getChildrenNumber(); ++ci) { m_entityContainer.getChild(ci)->addClipPlanes(negPlane); } } } }
bool HornRegistrationTools::FindAbsoluteOrientation(GenericCloud* lCloud, GenericCloud* rCloud, ScaledTransformation& trans, bool fixedScale/*=false*/) { //resulting transformation (R is invalid on initialization and T is (0,0,0)) trans.R.invalidate(); trans.T = CCVector3(0,0,0); trans.s = (PointCoordinateType)1.0; assert(rCloud && lCloud); if (!rCloud || !lCloud || rCloud->size() != lCloud->size() || rCloud->size()<3) return false; unsigned count = rCloud->size(); assert(count>2); //determine best scale? if (!fixedScale) { CCVector3 Gr = GeometricalAnalysisTools::computeGravityCenter(rCloud); CCVector3 Gl = GeometricalAnalysisTools::computeGravityCenter(lCloud); //we determine scale with the symmetrical form as proposed by Horn double lNorm2Sum = 0.0; { lCloud->placeIteratorAtBegining(); for (unsigned i=0;i<count;i++) { CCVector3 Pi = *lCloud->getNextPoint()-Gl; lNorm2Sum += Pi.dot(Pi); } } if (lNorm2Sum >= ZERO_TOLERANCE) { double rNorm2Sum = 0.0; { rCloud->placeIteratorAtBegining(); for (unsigned i=0;i<count;i++) { CCVector3 Pi = *rCloud->getNextPoint()-Gr; rNorm2Sum += Pi.dot(Pi); } } //resulting scale trans.s = (PointCoordinateType)sqrt(rNorm2Sum/lNorm2Sum); } //else //{ // //shouldn't happen! //} } return RegistrationProcedure(lCloud,rCloud,trans,0,0,trans.s); }
//return angle between two vectors (in degrees) //warning: vectors will be normalized by default double GetAngle_deg(CCVector3& AB, CCVector3& AC) { AB.normalize(); AC.normalize(); double dotprod = AB.dot(AC); if (dotprod<=-1.0) return 180.0; else if (dotprod>1.0) return 0.0; return 180.0*acos(dotprod)/M_PI; }
void CCMiscTools::ComputeBaseVectors(const CCVector3 &N, CCVector3& X, CCVector3& Y) { CCVector3 Nunit = N; Nunit.normalize(); //we create a first vector orthogonal to the input one X = Nunit.orthogonal(); //X is also normalized //we deduce the orthogonal vector to the input one and X Y = N.cross(X); //Y.normalize(); //should already be normalized! }
//return angle between two vectors (in degrees) //warning: vectors will be normalized by default static double GetAngle_deg(CCVector3 AB, CCVector3 AC) { AB.normalize(); AC.normalize(); double dotprod = AB.dot(AC); //clamp value (just in case) if (dotprod <= -1.0) dotprod = -1.0; else if (dotprod > 1.0) dotprod = 1.0; return acos(dotprod) * CC_RAD_TO_DEG; }
ccGLMatrix ccGLMatrix::zRotation() const { //we can use the standard Euler angles convention here float phi,theta,psi; CCVector3 T; getParameters(phi,theta,psi,T); assert(T.norm2()==0); ccGLMatrix newRotMat; newRotMat.initFromParameters(phi,0,0,T); return newRotMat; }
void ccClipBox::update() { if (!m_associatedEntity) { return; } #ifdef USE_OPENGL m_associatedEntity->removeAllClipPlanes(); //now add the 6 box clipping planes ccBBox extents; ccGLMatrix transformation; get(extents, transformation); CCVector3 C = transformation * extents.getCenter(); CCVector3 halfDim = extents.getDiagVec() / 2; //for each dimension for (unsigned d = 0; d < 3; ++d) { CCVector3 N = transformation.getColumnAsVec3D(d); //positive side { ccClipPlane posPlane; posPlane.equation.x = N.x; posPlane.equation.y = N.y; posPlane.equation.z = N.z; //compute the 'constant' coefficient knowing that P belongs to the plane if (P - (C - half_dim * N)).N = 0 posPlane.equation.w = -static_cast<double>(C.dot(N)) + halfDim.u[d]; m_associatedEntity->addClipPlanes(posPlane); } //negative side { ccClipPlane negPlane; negPlane.equation.x = -N.x; negPlane.equation.y = -N.y; negPlane.equation.z = -N.z; //compute the 'constant' coefficient knowing that P belongs to the plane if (P - (C + half_dim * N)).N = 0 //negPlane.equation.w = -(static_cast<double>(C.dot(N)) + halfDim.u[d]); negPlane.equation.w = static_cast<double>(C.dot(N)) + halfDim.u[d]; m_associatedEntity->addClipPlanes(negPlane); } } #else flagPointsInside(); #endif }
void ccGBLSensor::projectPoint( const CCVector3& sourcePoint, CCVector2& destPoint, PointCoordinateType &depth, double posIndex/*=0*/) const { //project point in sensor world CCVector3 P = sourcePoint; //sensor to world global transformation = sensor position * rigid transformation ccIndexedTransformation sensorPos; //identity by default if (m_posBuffer) m_posBuffer->getInterpolatedTransformation(posIndex,sensorPos); sensorPos *= m_rigidTransformation; //apply (inverse) global transformation (i.e world to sensor) sensorPos.inverse().apply(P); //convert to 2D sensor field of view + compute its distance switch (m_rotationOrder) { case YAW_THEN_PITCH: { //yaw = angle around Z, starting from 0 in the '+X' direction destPoint.x = atan2(P.y,P.x); //pitch = angle around the lateral axis, between -pi (-Z) to pi (+Z) by default destPoint.y = atan2(P.z,sqrt(P.x*P.x + P.y*P.y)); break; } case PITCH_THEN_YAW: { //FIXME //yaw = angle around Z, starting from 0 in the '+X' direction destPoint.x = -atan2(sqrt(P.y*P.y + P.z*P.z),P.x); //pitch = angle around the lateral axis, between -pi (-Z) to pi (+Z) by default destPoint.y = -atan2(P.y,P.z); break; } default: assert(false); } //if the yaw angles are shifted if (m_yawAnglesAreShifted && destPoint.x < 0) destPoint.x += static_cast<PointCoordinateType>(2.0*M_PI); //if the pitch angles are shifted if (m_pitchAnglesAreShifted && destPoint.y < 0) destPoint.y += static_cast<PointCoordinateType>(2.0*M_PI); depth = P.norm(); }
float ccFastMarchingForNormsDirection::computePropagationConfidence(DirectionCell* originCell, DirectionCell* destCell) const { //1) it depends on the angle between the current cell's orientation // and its neighbor's orientation (symmetric) //2) it depends on whether the neighbor's relative position is // compatible with the current cell orientation (symmetric) CCVector3 AB = destCell->C - originCell->C; AB.normalize(); float psOri = fabs(static_cast<float>(AB.dot(originCell->N))); //ideal: 90 degrees float psDest = fabs(static_cast<float>(AB.dot(destCell->N))); //ideal: 90 degrees float oriConfidence = (psOri + psDest)/2; //between 0 and 1 (ideal: 0) return 1.0f - oriConfidence; }
void CCTile3D::setPositionXYZ(const float x, const float y, const float z) { if( position.x != x || position.y != y || position.z != z ) { CCVector3 distance = position; super::setPositionXYZ( x, y, z ); distance.sub( position ); for( int i=0; i<attachments.length; ++i ) { CCSceneObject *attachment = attachments.list[i]; attachment->translate( -distance.x, -distance.y, -distance.z ); } CCOctreeRefreshObject( this ); } }
bool CharacterUpdaterPlayer::reportAttack(CCObject *attackedBy, const float force, const float damage, const float x, const float y, const float z) { if( health > 0.0f ) { health -= damage; } CCVector3 attackVelocity = CCVector3( x, y, z ); attackVelocity.unitize(); attackVelocity.mul( force * 0.1f ); player->incrementAdditionalVelocity( attackVelocity.x, attackVelocity.y, attackVelocity.z ); findNewAnchor = true; player->getGame()->registerAttack( attackedBy, player, damage ); return false; }
static bool lineCheckGetIntersection(const float dist1, const float dist2, const CCVector3 &point1, const CCVector3 &point2, CCVector3 &hitLocation) { if( ( dist1 * dist2 ) >= 0.0f ) { return false; } if( dist1 == dist2 ) { return false; } // point1 + ( point2 - point1 ) * ( -dst1 / ( dst2 - dst1 ) ); hitLocation = point2; hitLocation.sub( point1 ); hitLocation.mul( -dist1 / ( dist2 - dist1 ) ); hitLocation.add( point1 ); return true; }
float FastMarchingForFacetExtraction::computeTCoefApprox(CCLib::FastMarching::Cell* originCell, CCLib::FastMarching::Cell* destCell) const { PlanarCell* oCell = static_cast<PlanarCell*>(originCell); PlanarCell* dCell = static_cast<PlanarCell*>(destCell); //compute the 'confidence' relatively to the neighbor cell //1) it depends on the angle between the current cell's orientation // and its neighbor's orientation (symmetric) //2) it depends on whether the neighbor's relative position is // compatible with the current cell orientation (symmetric) float orientationConfidence = 0; { CCVector3 AB = dCell->C - oCell->C; AB.normalize(); float psOri = fabs(static_cast<float>(AB.dot(oCell->N))); //ideal: 90 degrees float psDest = fabs(static_cast<float>(AB.dot(dCell->N))); //ideal: 90 degrees orientationConfidence = (psOri + psDest)/2; //between 0 and 1 (ideal: 0) } //add reprojection error into balance if (m_useRetroProjectionError && m_octree && oCell->N.norm2() != 0) { PointCoordinateType theLSQPlaneEquation[4]; theLSQPlaneEquation[0] = oCell->N.x; theLSQPlaneEquation[1] = oCell->N.y; theLSQPlaneEquation[2] = oCell->N.z; theLSQPlaneEquation[3] = oCell->C.dot(oCell->N); CCLib::ReferenceCloud Yk(m_octree->associatedCloud()); if (m_octree->getPointsInCell(oCell->cellCode,m_gridLevel,&Yk,true)) { ScalarType reprojError = CCLib::DistanceComputationTools::ComputeCloud2PlaneDistance(&Yk,theLSQPlaneEquation,m_errorMeasure); if (reprojError >= 0) return (1.0f-orientationConfidence) * static_cast<float>(reprojError); } } return (1.0f-orientationConfidence) /** oCell->planarError*/; }
//helper: computes a facet horizontal and vertical extensions void ComputeFacetExtensions(CCVector3& N, ccPolyline* facetContour, double& horizExt, double& vertExt) { //horizontal and vertical extensions horizExt = vertExt = 0; CCLib::GenericIndexedCloudPersist* vertCloud = facetContour->getAssociatedCloud(); if (vertCloud) { //oriRotMat.applyRotation(N); //DGM: oriRotMat is only for display! //we assume that at this point the "up" direction is always (0,0,1) CCVector3 Xf(1,0,0), Yf(0,1,0); //we get the horizontal vector on the plane CCVector3 D = CCVector3(0,0,1).cross(N); if (D.norm2() > ZERO_TOLERANCE) //otherwise the facet is horizontal! { Yf = D; Yf.normalize(); Xf = N.cross(Yf); } const CCVector3* G = CCLib::Neighbourhood(vertCloud).getGravityCenter(); ccBBox box; for (unsigned i=0; i<vertCloud->size(); ++i) { const CCVector3 P = *(vertCloud->getPoint(i)) - *G; CCVector3 p( P.dot(Xf), P.dot(Yf), 0 ); box.add(p); } vertExt = box.getDiagVec().x; horizExt = box.getDiagVec().y; } }
void cc2DLabel::getLabelInfo3(LabelInfo3& info) const { info.cloud1 = info.cloud2 = info.cloud3 = 0; if (m_points.size() != 3) return; //1st point info.cloud1 = m_points[0].cloud; info.point1Index = m_points[0].index; const CCVector3* P1 = info.cloud1->getPointPersistentPtr(info.point1Index); //2nd point info.cloud2 = m_points[1].cloud; info.point2Index = m_points[1].index; const CCVector3* P2 = info.cloud2->getPointPersistentPtr(info.point2Index); //3rd point info.cloud3 = m_points[2].cloud; info.point3Index = m_points[2].index; const CCVector3* P3 = info.cloud3->getPointPersistentPtr(info.point3Index); //area CCVector3 P1P2 = *P2-*P1; CCVector3 P1P3 = *P3-*P1; CCVector3 P2P3 = *P3-*P2; CCVector3 N = P1P2.cross(P1P3); //N = ABxAC info.area = N.norm()/2; //normal N.normalize(); info.normal = N; //edges length info.edges.u[0] = P1P2.norm2d(); //edge 1-2 info.edges.u[1] = P2P3.norm2d(); //edge 2-3 info.edges.u[2] = P1P3.norm2d(); //edge 3-1 //angle info.angles.u[0] = GetAngle_deg(P1P2,P1P3); //angleAtP1 info.angles.u[1] = GetAngle_deg(P2P3,-P1P2); //angleAtP2 info.angles.u[2] = GetAngle_deg(-P1P3,-P2P3); //angleAtP3 (should be equal to 180-a1-a2!) }
bool GeometricalAnalysisTools::computeSphereFrom4( const CCVector3& A, const CCVector3& B, const CCVector3& C, const CCVector3& D, CCVector3& center, PointCoordinateType& radius ) { //inspired from 'tetrahedron_circumsphere_3d' by Adrian Bowyer and John Woodwark //Set up the linear system. double a[12]; { CCVector3 AB = B-A; a[0] = AB.x; a[3] = AB.y; a[6] = AB.z; a[9] = AB.norm2d(); } { CCVector3 AC = C-A; a[1] = AC.x; a[4] = AC.y; a[7] = AC.z; a[10] = AC.norm2d(); } { CCVector3 AD = D-A; a[2] = AD.x; a[5] = AD.y; a[8] = AD.z; a[11] = AD.norm2d(); } // Solve the linear system (with Gauss-Jordan elimination) if ( dmat_solve ( 3, 1, a ) != 0 ) { //system is singular? return false; } // Compute the radius and center. CCVector3 u = CCVector3(static_cast<PointCoordinateType>(a[0+3*3]), static_cast<PointCoordinateType>(a[1+3*3]), static_cast<PointCoordinateType>(a[2+3*3])) / 2; radius = u.norm(); center = A + u; return true; }
CCSceneCollideable* CCBasicLineCollisionCheck(const CCVector3 &start, const CCVector3 &end, const float width, CCSceneCollideable *source) { static CCVector3 hitLocation; CCSceneCollideable *hitObject = CCBasicLineCollisionCheck( gEngine->collisionManager.collideables.list, gEngine->collisionManager.collideables.length, source, start, end, &hitLocation, true, collision_static, true ); if( hitObject == NULL ) { static CCVector3 minStart, minEnd, maxStart, maxEnd; minStart.set( start.x - width, start.y, start.z - width ); minEnd.set( end.x - width, start.y, end.z - width ); maxStart.set( start.x + width, start.y, start.z + width ); maxEnd.set( end.x - width, start.y, end.z - width ); hitObject = CCBasicLineCollisionCheck( gEngine->collisionManager.collideables.list, gEngine->collisionManager.collideables.length, NULL, minStart, maxEnd, &hitLocation, true, collision_static, true ); if( hitObject == NULL ) { hitObject = CCBasicLineCollisionCheck( gEngine->collisionManager.collideables.list, gEngine->collisionManager.collideables.length, NULL, maxStart, minEnd, &hitLocation, true, collision_static, true ); } } return hitObject; }
void ccGLMatrix::getParameters(float& alpha, CCVector3& axis3D, CCVector3& t3D) const { float trace = R11 + R22 + R33; trace = 0.5f*(trace-1.0f); if (fabs(trace)<1.0) { alpha = acos(trace); if (alpha > (float)M_PI_2) alpha -= (float)M_PI; } else alpha = 0.0; axis3D.x = (float)(R32-R23); axis3D.y = (float)(R13-R31); axis3D.z = (float)(R21-R12); axis3D.normalize(); t3D.x = R14; t3D.y = R24; t3D.z = R34; }
void PCVContext::setViewDirection(const CCVector3& V) { if (!m_pixBuffer || !m_pixBuffer->isValid()) return; m_pixBuffer->makeCurrent(); glMatrixMode(GL_MODELVIEW); glPushMatrix(); glLoadIdentity(); CCVector3 U(0,0,1); if (1-fabs(V.dot(U)) < 1.0e-4) { U.y = 1; U.z = 0; } gluLookAt(-V.x,-V.y,-V.z,0.0,0.0,0.0,U.x,U.y,U.z); glGetFloatv(GL_MODELVIEW_MATRIX, m_viewMat); glPopMatrix(); }
void ccPolyline::drawMeOnly(CC_DRAW_CONTEXT& context) { //no picking enabled on polylines if (MACRO_DrawPointNames(context)) return; unsigned vertCount = size(); if (vertCount < 2) return; bool draw = false; if (MACRO_Draw3D(context)) { draw = !m_mode2D; } else if (m_mode2D) { bool drawFG = MACRO_Foreground(context); draw = ((drawFG && m_foreground) || (!drawFG && !m_foreground)); } if (draw) { //standard case: list names pushing bool pushName = MACRO_DrawEntityNames(context); if (pushName) glPushName(getUniqueIDForDisplay()); if (colorsShown()) ccGL::Color3v(m_rgbColor.rgb); //display polyline if (vertCount > 1) { if (m_width != 0) { glPushAttrib(GL_LINE_BIT); glLineWidth(static_cast<GLfloat>(m_width)); } //DGM: we do the 'GL_LINE_LOOP' manually as I have a strange bug //on one on my graphic card with this mode! //glBegin(m_isClosed ? GL_LINE_LOOP : GL_LINE_STRIP); glBegin(GL_LINE_STRIP); for (unsigned i=0; i<vertCount; ++i) { ccGL::Vertex3v(getPoint(i)->u); } if (m_isClosed) { ccGL::Vertex3v(getPoint(0)->u); } glEnd(); //display arrow if (m_showArrow && m_arrowIndex < vertCount && (m_arrowIndex > 0 || m_isClosed)) { const CCVector3* P0 = getPoint(m_arrowIndex == 0 ? vertCount-1 : m_arrowIndex-1); const CCVector3* P1 = getPoint(m_arrowIndex); //direction of the last polyline chunk CCVector3 u = *P1 - *P0; u.normalize(); if (m_mode2D) { u *= -m_arrowLength; static const PointCoordinateType s_defaultArrowAngle = static_cast<PointCoordinateType>(15.0 * CC_DEG_TO_RAD); static const PointCoordinateType cost = cos(s_defaultArrowAngle); static const PointCoordinateType sint = sin(s_defaultArrowAngle); CCVector3 A(cost * u.x - sint * u.y, sint * u.x + cost * u.y, 0); CCVector3 B(cost * u.x + sint * u.y, -sint * u.x + cost * u.y, 0); glBegin(GL_POLYGON); ccGL::Vertex3v((A+*P1).u); ccGL::Vertex3v((B+*P1).u); ccGL::Vertex3v(( *P1).u); glEnd(); } else { if (!c_unitArrow) { c_unitArrow = QSharedPointer<ccCone>(new ccCone(0.5,0.0,1.0)); c_unitArrow->showColors(true); c_unitArrow->showNormals(false); c_unitArrow->setVisible(true); c_unitArrow->setEnabled(true); } if (colorsShown()) c_unitArrow->setTempColor(m_rgbColor); else c_unitArrow->setTempColor(context.pointsDefaultCol); //build-up unit arrow own 'context' CC_DRAW_CONTEXT markerContext = context; markerContext.flags &= (~CC_DRAW_ENTITY_NAMES); //we must remove the 'push name flag' so that the sphere doesn't push its own! markerContext._win = 0; glMatrixMode(GL_MODELVIEW); glPushMatrix(); ccGL::Translate(P1->x,P1->y,P1->z); ccGLMatrix rotMat = ccGLMatrix::FromToRotation(CCVector3(0,0,1),u); glMultMatrixf(rotMat.inverse().data()); glScalef(m_arrowLength,m_arrowLength,m_arrowLength); ccGL::Translate(0.0,0.0,-0.5); c_unitArrow->draw(markerContext); glPopMatrix(); } } if (m_width != 0) { glPopAttrib(); } } //display vertices if (m_showVertices) { glPushAttrib(GL_POINT_BIT); glPointSize((GLfloat)m_vertMarkWidth); glBegin(GL_POINTS); for (unsigned i=0; i<vertCount; ++i) { ccGL::Vertex3v(getPoint(i)->u); } glEnd(); glPopAttrib(); } if (pushName) glPopName(); } }
ccQuadric* ccQuadric::Fit(CCLib::GenericIndexedCloudPersist *cloud, double* rms/*=0*/) { //number of points unsigned count = cloud->size(); if (count < CC_LOCAL_MODEL_MIN_SIZE[HF]) { ccLog::Warning(QString("[ccQuadric::fitTo] Not enough points in input cloud to fit a quadric! (%1 at the very least are required)").arg(CC_LOCAL_MODEL_MIN_SIZE[HF])); return 0; } //project the points on a 2D plane CCVector3 G, X, Y, N; { CCLib::Neighbourhood Yk(cloud); //plane equation const PointCoordinateType* theLSQPlane = Yk.getLSQPlane(); if (!theLSQPlane) { ccLog::Warning("[ccQuadric::Fit] Not enough points to fit a quadric!"); return 0; } assert(Yk.getGravityCenter()); G = *Yk.getGravityCenter(); //local base N = CCVector3(theLSQPlane); assert(Yk.getLSQPlaneX() && Yk.getLSQPlaneY()); X = *Yk.getLSQPlaneX(); //main direction Y = *Yk.getLSQPlaneY(); //secondary direction } //project the points in a temporary cloud ccPointCloud tempCloud("temporary"); if (!tempCloud.reserve(count)) { ccLog::Warning("[ccQuadric::Fit] Not enough memory!"); return 0; } cloud->placeIteratorAtBegining(); for (unsigned k=0; k<count; ++k) { //projection into local 2D plane ref. CCVector3 P = *(cloud->getNextPoint()) - G; tempCloud.addPoint(CCVector3(P.dot(X),P.dot(Y),P.dot(N))); } CCLib::Neighbourhood Zk(&tempCloud); { //set exact values for gravity center and plane equation //(just to be sure and to avoid re-computing them) Zk.setGravityCenter(CCVector3(0,0,0)); PointCoordinateType perfectEq[4] = { 0, 0, 1, 0 }; Zk.setLSQPlane( perfectEq, CCVector3(1,0,0), CCVector3(0,1,0), CCVector3(0,0,1)); } uchar hfdims[3]; const PointCoordinateType* eq = Zk.getHeightFunction(hfdims); if (!eq) { ccLog::Warning("[ccQuadric::Fit] Failed to fit a quadric!"); return 0; } //we recenter the quadric object ccGLMatrix glMat(X,Y,N,G); ccBBox bb = tempCloud.getBB(); CCVector2 minXY(bb.minCorner().x,bb.minCorner().y); CCVector2 maxXY(bb.maxCorner().x,bb.maxCorner().y); ccQuadric* quadric = new ccQuadric(minXY, maxXY, eq, hfdims, &glMat); quadric->setMetaData(QString("Equation"),QVariant(quadric->getEquationString())); //compute rms if necessary if (rms) { const uchar& dX = hfdims[0]; const uchar& dY = hfdims[1]; const uchar& dZ = hfdims[2]; *rms = 0; for (unsigned k=0; k<count; ++k) { //projection into local 2D plane ref. const CCVector3* P = tempCloud.getPoint(k); PointCoordinateType z = eq[0] + eq[1]*P->u[dX] + eq[2]*P->u[dY] + eq[3]*P->u[dX]*P->u[dX] + eq[4]*P->u[dX]*P->u[dY] + eq[5]*P->u[dY]*P->u[dY]; *rms += (z-P->z)*(z-P->z); } if (count) { *rms = sqrt(*rms / count); quadric->setMetaData(QString("RMS"),QVariant(*rms)); } } return quadric; }
ccGBLSensor::NormalGrid* ccGBLSensor::projectNormals( CCLib::GenericCloud* cloud, const NormalGrid& theNorms, double posIndex/*=0*/) const { if (!cloud || !theNorms.isAllocated()) return 0; unsigned size = m_depthBuffer.height*m_depthBuffer.width; if (size == 0) return 0; //depth buffer empty/not initialized! NormalGrid* normalGrid = new NormalGrid; if (!normalGrid->resize(size,0)) return 0; //not enough memory //sensor to world global transformation = sensor position * rigid transformation ccIndexedTransformation sensorPos; //identity by default if (m_posBuffer) m_posBuffer->getInterpolatedTransformation(posIndex,sensorPos); sensorPos *= m_rigidTransformation; //poject each point + normal { cloud->placeIteratorAtBegining(); unsigned pointCount = cloud->size(); for (unsigned i=0; i<pointCount; ++i) { const CCVector3* P = cloud->getNextPoint(); const PointCoordinateType* N = theNorms.getValue(i); //project point CCVector2 Q; PointCoordinateType depth1; projectPoint(*P,Q,depth1,m_activeIndex); CCVector3 S; CCVector3 U = *P - sensorPos.getTranslationAsVec3D(); PointCoordinateType distToSensor = U.norm(); if (distToSensor > ZERO_TOLERANCE) { //normal component along sensor viewing dir. S.z = -CCVector3::vdot(N,U.u)/distToSensor; if (S.z > 1.0-ZERO_TOLERANCE) { S.x = 0; S.y = 0; } else { //and point+normal CCVector3 P2 = *P + CCVector3(N); CCVector2 S2; PointCoordinateType depth2; projectPoint(P2,S2,depth2,m_activeIndex); //deduce other normals components PointCoordinateType coef = sqrt((1 - S.z*S.z)/(S.x*S.x + S.y*S.y)); S.x = coef * (S2.x - Q.x); S.y = coef * (S2.y - Q.y); } } else { S = CCVector3(N); } //project in Z-buffer unsigned x,y; if (convertToDepthMapCoords(Q.x,Q.y,x,y)) { //add the transformed normal PointCoordinateType* newN = normalGrid->getValue(y*m_depthBuffer.width + x); CCVector3::vadd(newN,S.u,newN); } else { //shouldn't happen! assert(false); } } } //normalize { normalGrid->placeIteratorAtBegining(); for (unsigned i=0; i<m_depthBuffer.height*m_depthBuffer.width; ++i) { PointCoordinateType* newN = normalGrid->getCurrentValue(); CCVector3::vnormalize(newN); normalGrid->forwardIterator(); } } return normalGrid; }
void qRansacSD::doAction() { assert(m_app); if (!m_app) return; const ccHObject::Container& selectedEntities = m_app->getSelectedEntities(); size_t selNum = selectedEntities.size(); if (selNum!=1) { m_app->dispToConsole("Select only one cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE); return; } ccHObject* ent = selectedEntities[0]; assert(ent); if (!ent || !ent->isA(CC_POINT_CLOUD)) { m_app->dispToConsole("Select a real point cloud!",ccMainAppInterface::ERR_CONSOLE_MESSAGE); return; } ccPointCloud* pc = static_cast<ccPointCloud*>(ent); //input cloud size_t count = (size_t)pc->size(); bool hasNorms = pc->hasNormals(); PointCoordinateType bbMin[3],bbMax[3]; pc->getBoundingBox(bbMin,bbMax); //Convert CC point cloud to RANSAC_SD type PointCloud cloud; { try { cloud.reserve(count); } catch(...) { m_app->dispToConsole("Not enough memory!",ccMainAppInterface::ERR_CONSOLE_MESSAGE); return; } //default point & normal Point Pt; Pt.normal[0] = 0.0; Pt.normal[1] = 0.0; Pt.normal[2] = 0.0; for (unsigned i=0; i<(unsigned)count; ++i) { const CCVector3* P = pc->getPoint(i); Pt.pos[0] = P->x; Pt.pos[1] = P->y; Pt.pos[2] = P->z; if (hasNorms) { const PointCoordinateType* N = pc->getPointNormal(i); Pt.normal[0] = N[0]; Pt.normal[1] = N[1]; Pt.normal[2] = N[2]; } cloud.push_back(Pt); } //manually set bounding box! Vec3f cbbMin,cbbMax; cbbMin[0] = bbMin[0]; cbbMin[1] = bbMin[1]; cbbMin[2] = bbMin[2]; cbbMax[0] = bbMax[0]; cbbMax[1] = bbMax[1]; cbbMax[2] = bbMax[2]; cloud.setBBox(cbbMin,cbbMax); } //cloud scale (useful for setting several parameters const float scale = cloud.getScale(); //init dialog with default values ccRansacSDDlg rsdDlg(m_app->getMainWindow()); rsdDlg.epsilonDoubleSpinBox->setValue(.01f * scale); // set distance threshold to .01f of bounding box width // NOTE: Internally the distance threshold is taken as 3 * ransacOptions.m_epsilon!!! rsdDlg.bitmapEpsilonDoubleSpinBox->setValue(.02f * scale); // set bitmap resolution to .02f of bounding box width // NOTE: This threshold is NOT multiplied internally! rsdDlg.supportPointsSpinBox->setValue(s_supportPoints); rsdDlg.normThreshDoubleSpinBox->setValue(s_normThresh); rsdDlg.probaDoubleSpinBox->setValue(s_proba); rsdDlg.planeCheckBox->setChecked(s_primEnabled[0]); rsdDlg.sphereCheckBox->setChecked(s_primEnabled[1]); rsdDlg.cylinderCheckBox->setChecked(s_primEnabled[2]); rsdDlg.coneCheckBox->setChecked(s_primEnabled[3]); rsdDlg.torusCheckBox->setChecked(s_primEnabled[4]); if (!rsdDlg.exec()) return; //for parameters persistence { s_supportPoints = rsdDlg.supportPointsSpinBox->value(); s_normThresh = rsdDlg.normThreshDoubleSpinBox->value(); s_proba = rsdDlg.probaDoubleSpinBox->value(); //consistency check { unsigned char primCount = 0; for (unsigned char k=0; k<5; ++k) primCount += (unsigned)s_primEnabled[k]; if (primCount==0) { m_app->dispToConsole("No primitive type selected!",ccMainAppInterface::ERR_CONSOLE_MESSAGE); return; } } s_primEnabled[0] = rsdDlg.planeCheckBox->isChecked(); s_primEnabled[1] = rsdDlg.sphereCheckBox->isChecked(); s_primEnabled[2] = rsdDlg.cylinderCheckBox->isChecked(); s_primEnabled[3] = rsdDlg.coneCheckBox->isChecked(); s_primEnabled[4] = rsdDlg.torusCheckBox->isChecked(); } //import parameters from dialog RansacShapeDetector::Options ransacOptions; { ransacOptions.m_epsilon = rsdDlg.epsilonDoubleSpinBox->value(); ransacOptions.m_bitmapEpsilon = rsdDlg.bitmapEpsilonDoubleSpinBox->value(); ransacOptions.m_normalThresh = rsdDlg.normThreshDoubleSpinBox->value(); ransacOptions.m_minSupport = rsdDlg.supportPointsSpinBox->value(); ransacOptions.m_probability = rsdDlg.probaDoubleSpinBox->value(); } //progress dialog ccProgressDialog progressCb(false,m_app->getMainWindow()); progressCb.setRange(0,0); if (!hasNorms) { progressCb.setInfo("Computing normals (please wait)"); progressCb.start(); QApplication::processEvents(); cloud.calcNormals(.01f * scale); if (pc->reserveTheNormsTable()) { for (size_t i=0; i<count; ++i) { CCVector3 N(cloud[i].normal); N.normalize(); pc->addNorm(N.u); } pc->showNormals(true); //currently selected entities appearance may have changed! pc->prepareDisplayForRefresh_recursive(); } else { m_app->dispToConsole("Not enough memory to compute normals!",ccMainAppInterface::ERR_CONSOLE_MESSAGE); return; } } // set which primitives are to be detected by adding the respective constructors RansacShapeDetector detector(ransacOptions); // the detector object if (rsdDlg.planeCheckBox->isChecked()) detector.Add(new PlanePrimitiveShapeConstructor()); if (rsdDlg.sphereCheckBox->isChecked()) detector.Add(new SpherePrimitiveShapeConstructor()); if (rsdDlg.cylinderCheckBox->isChecked()) detector.Add(new CylinderPrimitiveShapeConstructor()); if (rsdDlg.coneCheckBox->isChecked()) detector.Add(new ConePrimitiveShapeConstructor()); if (rsdDlg.torusCheckBox->isChecked()) detector.Add(new TorusPrimitiveShapeConstructor()); size_t remaining = count; typedef std::pair< MiscLib::RefCountPtr< PrimitiveShape >, size_t > DetectedShape; MiscLib::Vector< DetectedShape > shapes; // stores the detected shapes // run detection // returns number of unassigned points // the array shapes is filled with pointers to the detected shapes // the second element per shapes gives the number of points assigned to that primitive (the support) // the points belonging to the first shape (shapes[0]) have been sorted to the end of pc, // i.e. into the range [ pc.size() - shapes[0].second, pc.size() ) // the points of shape i are found in the range // [ pc.size() - \sum_{j=0..i} shapes[j].second, pc.size() - \sum_{j=0..i-1} shapes[j].second ) { progressCb.setInfo("Operation in progress"); progressCb.setMethodTitle("Ransac Shape Detection"); progressCb.start(); //run in a separate thread s_detector = &detector; s_shapes = &shapes; s_cloud = &cloud; QFuture<void> future = QtConcurrent::run(doDetection); unsigned progress = 0; while (!future.isFinished()) { #if defined(_WIN32) || defined(WIN32) ::Sleep(500); #else sleep(500); #endif progressCb.update(++progress); //Qtconcurrent::run can't be canceled! /*if (progressCb.isCancelRequested()) { future.cancel(); future.waitForFinished(); s_remainingPoints = count; break; } //*/ } remaining = s_remainingPoints; progressCb.stop(); QApplication::processEvents(); } //else //{ // remaining = detector.Detect(cloud, 0, cloud.size(), &shapes); //} #ifdef _DEBUG FILE* fp = fopen("RANS_SD_trace.txt","wt"); fprintf(fp,"[Options]\n"); fprintf(fp,"epsilon=%f\n",ransacOptions.m_epsilon); fprintf(fp,"bitmap epsilon=%f\n",ransacOptions.m_bitmapEpsilon); fprintf(fp,"normal thresh=%f\n",ransacOptions.m_normalThresh); fprintf(fp,"min support=%i\n",ransacOptions.m_minSupport); fprintf(fp,"probability=%f\n",ransacOptions.m_probability); fprintf(fp,"\n[Statistics]\n"); fprintf(fp,"input points=%i\n",count); fprintf(fp,"segmented=%i\n",count-remaining); fprintf(fp,"remaining=%i\n",remaining); if (shapes.size()>0) { fprintf(fp,"\n[Shapes]\n"); for (unsigned i=0; i<shapes.size(); ++i) { PrimitiveShape* shape = shapes[i].first; unsigned shapePointsCount = shapes[i].second; std::string desc; shape->Description(&desc); fprintf(fp,"#%i - %s - %i points\n",i+1,desc.c_str(),shapePointsCount); } } fclose(fp); #endif if (remaining == count) { m_app->dispToConsole("Segmentation failed...",ccMainAppInterface::ERR_CONSOLE_MESSAGE); return; } if (shapes.size() > 0) { ccHObject* group = 0; for (MiscLib::Vector<DetectedShape>::const_iterator it = shapes.begin(); it != shapes.end(); ++it) { const PrimitiveShape* shape = it->first; size_t shapePointsCount = it->second; //too many points?! if (shapePointsCount > count) { m_app->dispToConsole("Inconsistent result!",ccMainAppInterface::ERR_CONSOLE_MESSAGE); break; } std::string desc; shape->Description(&desc); //new cloud for sub-part ccPointCloud* pcShape = new ccPointCloud(desc.c_str()); //we fill cloud with sub-part points if (!pcShape->reserve((unsigned)shapePointsCount)) { m_app->dispToConsole("Not enough memory!",ccMainAppInterface::ERR_CONSOLE_MESSAGE); delete pcShape; break; } bool saveNormals = pcShape->reserveTheNormsTable(); for (size_t j=0; j<shapePointsCount; ++j) { pcShape->addPoint(CCVector3(cloud[count-1-j].pos)); if (saveNormals) pcShape->addNorm(cloud[count-1-j].normal); } //random color unsigned char col[3]= { (unsigned char)(255.0*(float)rand()/(float)RAND_MAX), (unsigned char)(255.0*(float)rand()/(float)RAND_MAX), 0 }; col[2]=255-(col[1]+col[2])/2; pcShape->setRGBColor(col); pcShape->showColors(true); pcShape->showNormals(saveNormals); pcShape->setVisible(true); //convert detected primitive into a CC primitive type ccGenericPrimitive* prim = 0; switch(shape->Identifier()) { case 0: //plane { const PlanePrimitiveShape* plane = static_cast<const PlanePrimitiveShape*>(shape); Vec3f G = plane->Internal().getPosition(); Vec3f N = plane->Internal().getNormal(); Vec3f X = plane->getXDim(); Vec3f Y = plane->getYDim(); //we look for real plane extents float minX,maxX,minY,maxY; for (size_t j=0; j<shapePointsCount; ++j) { std::pair<float,float> param; plane->Parameters(cloud[count-1-j].pos,¶m); if (j!=0) { if (minX<param.first) minX=param.first; else if (maxX>param.first) maxX=param.first; if (minY<param.second) minY=param.second; else if (maxY>param.second) maxY=param.second; } else { minX=maxX=param.first; minY=maxY=param.second; } } //we recenter plane (as it is not always the case!) float dX = maxX-minX; float dY = maxY-minY; G += X * (minX+dX*0.5); G += Y * (minY+dY*0.5); //we build matrix from these vecctors ccGLMatrix glMat(CCVector3(X.getValue()), CCVector3(Y.getValue()), CCVector3(N.getValue()), CCVector3(G.getValue())); //plane primitive prim = new ccPlane(dX,dY,&glMat); } break; case 1: //sphere { const SpherePrimitiveShape* sphere = static_cast<const SpherePrimitiveShape*>(shape); float radius = sphere->Internal().Radius(); Vec3f CC = sphere->Internal().Center(); pcShape->setName(QString("Sphere (r=%1)").arg(radius,0,'f')); //we build matrix from these vecctors ccGLMatrix glMat; glMat.setTranslation(CCVector3(CC.getValue())); //sphere primitive prim = new ccSphere(radius,&glMat); prim->setEnabled(false); } break; case 2: //cylinder { const CylinderPrimitiveShape* cyl = static_cast<const CylinderPrimitiveShape*>(shape); Vec3f G = cyl->Internal().AxisPosition(); Vec3f N = cyl->Internal().AxisDirection(); Vec3f X = cyl->Internal().AngularDirection(); Vec3f Y = N.cross(X); float r = cyl->Internal().Radius(); float hMin = cyl->MinHeight(); float hMax = cyl->MaxHeight(); float h = hMax-hMin; G += N * (hMin+h*0.5); pcShape->setName(QString("Cylinder (r=%1/h=%2)").arg(r,0,'f').arg(h,0,'f')); //we build matrix from these vecctors ccGLMatrix glMat(CCVector3(X.getValue()), CCVector3(Y.getValue()), CCVector3(N.getValue()), CCVector3(G.getValue())); //cylinder primitive prim = new ccCylinder(r,h,&glMat); prim->setEnabled(false); } break; case 3: //cone { const ConePrimitiveShape* cone = static_cast<const ConePrimitiveShape*>(shape); Vec3f CC = cone->Internal().Center(); Vec3f CA = cone->Internal().AxisDirection(); float alpha = cone->Internal().Angle(); //compute max height CCVector3 maxP(CC.getValue()); float maxHeight = 0; for (size_t j=0; j<shapePointsCount; ++j) { float h = cone->Internal().Height(cloud[count-1-j].pos); if (h>maxHeight) { maxHeight=h; maxP = CCVector3(cloud[count-1-j].pos); } } pcShape->setName(QString("Cone (alpha=%1/h=%2)").arg(alpha,0,'f').arg(maxHeight,0,'f')); float radius = tan(alpha)*maxHeight; CCVector3 Z = CCVector3(CA.getValue()); CCVector3 C = CCVector3(CC.getValue()); //cone apex //construct remaining of base Z.normalize(); CCVector3 X = maxP - (C + maxHeight * Z); X.normalize(); CCVector3 Y = Z * X; //we build matrix from these vecctors ccGLMatrix glMat(X,Y,Z,C+(maxHeight*0.5)*Z); //cone primitive prim = new ccCone(0,radius,maxHeight,0,0,&glMat); prim->setEnabled(false); } break; case 4: //torus { const TorusPrimitiveShape* torus = static_cast<const TorusPrimitiveShape*>(shape); if (torus->Internal().IsAppleShaped()) { m_app->dispToConsole("[qRansacSD] Apple-shaped torus are not handled by CloudCompare!",ccMainAppInterface::WRN_CONSOLE_MESSAGE); } else { Vec3f CC = torus->Internal().Center(); Vec3f CA = torus->Internal().AxisDirection(); float minRadius = torus->Internal().MinorRadius(); float maxRadius = torus->Internal().MajorRadius(); pcShape->setName(QString("Torus (r=%1/R=%2)").arg(minRadius,0,'f').arg(maxRadius,0,'f')); CCVector3 Z = CCVector3(CA.getValue()); CCVector3 C = CCVector3(CC.getValue()); //construct remaining of base CCVector3 X = Z.orthogonal(); CCVector3 Y = Z * X; //we build matrix from these vecctors ccGLMatrix glMat(X,Y,Z,C); //torus primitive prim = new ccTorus(maxRadius-minRadius,maxRadius+minRadius,M_PI*2.0,false,0,&glMat); prim->setEnabled(false); } } break; } //is there a primitive to add to part cloud? if (prim) { prim->applyGLTransformation_recursive(); pcShape->addChild(prim); prim->setDisplay(pcShape->getDisplay()); prim->setColor(col); prim->showColors(true); prim->setVisible(true); } if (!group) { group = new ccHObject(QString("Ransac Detected Shapes (%1)").arg(ent->getName())); m_app->addToDB(group,true,0,false); } group->addChild(pcShape); m_app->addToDB(pcShape,true,0,false); count -= shapePointsCount; QApplication::processEvents(); } if (group) { assert(group->getChildrenNumber()!=0); //we hide input cloud pc->setEnabled(false); m_app->dispToConsole("[qRansacSD] Input cloud has been automtically hidden!",ccMainAppInterface::WRN_CONSOLE_MESSAGE); //we add new group to DB/display group->setVisible(true); group->setDisplay_recursive(pc->getDisplay()); group->prepareDisplayForRefresh_recursive(); m_app->refreshAll(); } } }
ccPlane* ccPlane::Fit(CCLib::GenericIndexedCloudPersist *cloud, double* rms/*=0*/) { //number of points unsigned count = cloud->size(); if (count < 3) { ccLog::Warning("[ccPlane::Fit] Not enough points in input cloud to fit a plane!"); return 0; } CCLib::Neighbourhood Yk(cloud); //plane equation const PointCoordinateType* theLSPlane = Yk.getLSPlane(); if (!theLSPlane) { ccLog::Warning("[ccPlane::Fit] Not enough points to fit a plane!"); return 0; } //get the centroid const CCVector3* G = Yk.getGravityCenter(); assert(G); //and a local base CCVector3 N(theLSPlane); const CCVector3* X = Yk.getLSPlaneX(); //main direction assert(X); CCVector3 Y = N * (*X); //compute bounding box in 2D plane CCVector2 minXY(0,0), maxXY(0,0); cloud->placeIteratorAtBegining(); for (unsigned k=0; k<count; ++k) { //projection into local 2D plane ref. CCVector3 P = *(cloud->getNextPoint()) - *G; CCVector2 P2D( P.dot(*X), P.dot(Y) ); if (k != 0) { if (minXY.x > P2D.x) minXY.x = P2D.x; else if (maxXY.x < P2D.x) maxXY.x = P2D.x; if (minXY.y > P2D.y) minXY.y = P2D.y; else if (maxXY.y < P2D.y) maxXY.y = P2D.y; } else { minXY = maxXY = P2D; } } //we recenter the plane PointCoordinateType dX = maxXY.x-minXY.x; PointCoordinateType dY = maxXY.y-minXY.y; CCVector3 Gt = *G + *X * (minXY.x + dX / 2) + Y * (minXY.y + dY / 2); ccGLMatrix glMat(*X,Y,N,Gt); ccPlane* plane = new ccPlane(dX, dY, &glMat); //compute least-square fitting RMS if requested if (rms) { *rms = CCLib::DistanceComputationTools::computeCloud2PlaneDistanceRMS(cloud, theLSPlane); plane->setMetaData(QString("RMS"),QVariant(*rms)); } return plane; }
ccPlane* ccPlane::Fit(CCLib::GenericIndexedCloudPersist *cloud, double* rms/*=0*/) { //number of points unsigned count = cloud->size(); if (count < 3) { ccLog::Warning("[ccPlane::fitTo] Not enough points in input cloud to fit a plane!"); return 0; } CCLib::Neighbourhood Yk(cloud); //plane equation const PointCoordinateType* theLSQPlane = Yk.getLSQPlane(); if (!theLSQPlane) { ccLog::Warning("[ccGenericPointCloud::fitPlane] Not enough points to fit a plane!"); return 0; } //compute least-square fitting RMS if requested if (rms) { *rms = CCLib::DistanceComputationTools::computeCloud2PlaneDistanceRMS(cloud, theLSQPlane); } //get the centroid const CCVector3* G = Yk.getGravityCenter(); assert(G); //and a local base CCVector3 N(theLSQPlane); const CCVector3* X = Yk.getLSQPlaneX(); //main direction assert(X); CCVector3 Y = N * (*X); PointCoordinateType minX=0,maxX=0,minY=0,maxY=0; cloud->placeIteratorAtBegining(); for (unsigned k=0; k<count; ++k) { //projetion into local 2D plane ref. CCVector3 P = *(cloud->getNextPoint()) - *G; PointCoordinateType x2D = P.dot(*X); PointCoordinateType y2D = P.dot(Y); if (k!=0) { if (minX < x2D) minX = x2D; else if (maxX > x2D) maxX = x2D; if (minY < y2D) minY = y2D; else if (maxY > y2D) maxY = y2D; } else { minX = maxX = x2D; minY = maxY = y2D; } } //we recenter the plane PointCoordinateType dX = maxX-minX; PointCoordinateType dY = maxY-minY; CCVector3 Gt = *G + *X * (minX + dX*static_cast<PointCoordinateType>(0.5)); Gt += Y * (minY + dY*static_cast<PointCoordinateType>(0.5)); ccGLMatrix glMat(*X,Y,N,Gt); ccPlane* plane = new ccPlane(dX, dY, &glMat); return plane; }
ccPlane* ccGenericPointCloud::fitPlane(double* rms /*= 0*/) { //number of points unsigned count = size(); if (count<3) return 0; CCLib::Neighbourhood Yk(this); //we determine plane normal by computing the smallest eigen value of M = 1/n * S[(p-µ)*(p-µ)'] CCLib::SquareMatrixd eig = Yk.computeCovarianceMatrix().computeJacobianEigenValuesAndVectors(); //invalid matrix? if (!eig.isValid()) { //ccConsole::Warning("[ccPointCloud::fitPlane] Failed to compute plane/normal for cloud '%s'",getName()); return 0; } eig.sortEigenValuesAndVectors(); //plane equation PointCoordinateType theLSQPlane[4]; //the smallest eigen vector corresponds to the "least square best fitting plane" normal double vec[3]; eig.getEigenValueAndVector(2,vec); //PointCoordinateType sign = (vec[2] < 0.0 ? -1.0 : 1.0); //plane normal (always with a positive 'Z' by default) for (unsigned i=0;i<3;++i) theLSQPlane[i]=/*sign*/(PointCoordinateType)vec[i]; CCVector3 N(theLSQPlane); //we also get centroid const CCVector3* G = Yk.getGravityCenter(); assert(G); //eventually we just have to compute 'constant' coefficient a3 //we use the fact that the plane pass through the centroid --> GM.N = 0 (scalar prod) //i.e. a0*G[0]+a1*G[1]+a2*G[2]=a3 theLSQPlane[3] = G->dot(N); //least-square fitting RMS if (rms) { placeIteratorAtBegining(); *rms = 0.0; for (unsigned k=0;k<count;++k) { double d = (double)CCLib::DistanceComputationTools::computePoint2PlaneDistance(getNextPoint(),theLSQPlane); *rms += d*d; } *rms = sqrt(*rms)/(double)count; } //we has a plane primitive to the cloud eig.getEigenValueAndVector(0,vec); //main direction CCVector3 X(vec[0],vec[1],vec[2]); //plane normal //eig.getEigenValueAndVector(1,vec); //intermediate direction //CCVector3 Y(vec[0],vec[1],vec[2]); //plane normal CCVector3 Y = N * X; //we eventually check for plane extents PointCoordinateType minX=0.0,maxX=0.0,minY=0.0,maxY=0.0; placeIteratorAtBegining(); for (unsigned k=0;k<count;++k) { //projetion into local 2D plane ref. CCVector3 P = *getNextPoint() - *G; PointCoordinateType x2D = P.dot(X); PointCoordinateType y2D = P.dot(Y); if (k!=0) { if (minX<x2D) minX=x2D; else if (maxX>x2D) maxX=x2D; if (minY<y2D) minY=y2D; else if (maxY>y2D) maxY=y2D; } else { minX=maxX=x2D; minY=maxY=y2D; } } //we recenter plane (as it is not always the case!) float dX = maxX-minX; float dY = maxY-minY; CCVector3 Gt = *G + X * (minX+dX*0.5); Gt += Y * (minY+dY*0.5); ccGLMatrix glMat(X,Y,N,Gt); return new ccPlane(dX,dY,&glMat); }
SimpleCloud* MeshSamplingTools::samplePointsOnMesh(GenericMesh* theMesh, double samplingDensity, unsigned theoricNumberOfPoints, GenericProgressCallback* progressCb, GenericChunkedArray<1,unsigned>* triIndices/*=0*/) { assert(theMesh); unsigned triCount = (theMesh ? theMesh->size() : 0); if (triCount==0) return 0; if (theoricNumberOfPoints < 1) return 0; SimpleCloud* sampledCloud = new SimpleCloud(); if (!sampledCloud->reserve(theoricNumberOfPoints)) //not enough memory { delete sampledCloud; return 0; } if (triIndices) { triIndices->clear(); //not enough memory? DGM TODO: we should warn the caller if (!triIndices->reserve(theoricNumberOfPoints) || triIndices->capacity() < theoricNumberOfPoints) { delete sampledCloud; triIndices->clear(); return 0; } } NormalizedProgress* normProgress=0; if(progressCb) { normProgress = new NormalizedProgress(progressCb,triCount); progressCb->setMethodTitle("Mesh sampling"); char buffer[256]; sprintf(buffer,"Triangles: %i\nPoints: %i",triCount,theoricNumberOfPoints); progressCb->setInfo(buffer); progressCb->reset(); progressCb->start(); } unsigned addedPoints=0; //for each triangle theMesh->placeIteratorAtBegining(); for (unsigned n=0;n<triCount;++n) { const GenericTriangle* tri = theMesh->_getNextTriangle(); //summits (OAB) const CCVector3 *O = tri->_getA(); const CCVector3 *A = tri->_getB(); const CCVector3 *B = tri->_getC(); //edges (OA and OB) CCVector3 u = *A - *O; CCVector3 v = *B - *O; //we compute the (twice) the triangle area CCVector3 N = u.cross(v); double S = N.norm()/2.0; //we deduce the number of points to generate on this face double fPointsToAdd = S*samplingDensity; unsigned pointsToAdd = static_cast<unsigned>(fPointsToAdd); //if the face area is smaller than the surface/random point if (pointsToAdd == 0) { //we add a point with the same probability as its (relative) area if (static_cast<double>(rand()) <= fPointsToAdd * static_cast<double>(RAND_MAX)) pointsToAdd = 1; } if (pointsToAdd) { if (addedPoints + pointsToAdd >= theoricNumberOfPoints) { theoricNumberOfPoints+=pointsToAdd; if (!sampledCloud->reserve(theoricNumberOfPoints) || (triIndices && triIndices->capacity() < theoricNumberOfPoints && !triIndices->reserve(theoricNumberOfPoints))) //not enough memory { delete sampledCloud; sampledCloud = 0; triIndices->clear(); break; } } for (unsigned i=0; i<pointsToAdd; ++i) { //we generates random points as in: //'Greg Turk. Generating random points in triangles. In A. S. Glassner, editor,Graphics Gems, pages 24-28. Academic Press, 1990.' double x = static_cast<double>(rand())/static_cast<double>(RAND_MAX); double y = static_cast<double>(rand())/static_cast<double>(RAND_MAX); //we test if the generated point lies on the right side of (AB) if (x+y > 1.0) { x = 1.0-x; y = 1.0-y; } CCVector3 P = (*O) + static_cast<PointCoordinateType>(x) * u + static_cast<PointCoordinateType>(y) * v; sampledCloud->addPoint(P); if (triIndices) triIndices->addElement(n); ++addedPoints; } } if (normProgress && !normProgress->oneStep()) break; } if (normProgress) { delete normProgress; normProgress = 0; } if (sampledCloud) //can be in case of memory overflow! { if (addedPoints) { sampledCloud->resize(addedPoints); //should always be ok as addedPoints<theoricNumberOfPoints if (triIndices) triIndices->resize(addedPoints); } else { delete sampledCloud; sampledCloud = 0; if (triIndices) triIndices->clear(); } } return sampledCloud; }
//! Intersects this grid with a mesh bool ChamferDistanceTransformSigned::initDT(GenericIndexedMesh* mesh, PointCoordinateType cellLength, const CCVector3& gridMinCorner, GenericProgressCallback* progressCb/*=0*/) { if (!mesh || !isInitialized()) { assert(false); return false; } //cell dimension PointCoordinateType halfCellSize = cellLength / 2; std::vector<CellToTest> cellsToTest(1); //initial size must be > 0 unsigned cellsToTestCount = 0; //number of triangles unsigned numberOfTriangles = mesh->size(); //progress notification NormalizedProgress nProgress(progressCb, numberOfTriangles); if (progressCb) { char buffer[64]; sprintf(buffer, "Triangles: %u", numberOfTriangles); progressCb->reset(); progressCb->setInfo(buffer); progressCb->setMethodTitle("Init Distance Transform"); progressCb->start(); } //for each triangle: look for intersecting cells mesh->placeIteratorAtBegining(); for (unsigned n = 0; n<numberOfTriangles; ++n) { //get the positions (in the grid) of each vertex const GenericTriangle* T = mesh->_getNextTriangle(); //current triangle vertices const CCVector3* triPoints[3] = { T->_getA(), T->_getB(), T->_getC() }; CCVector3 AB = (*triPoints[1]) - (*triPoints[0]); CCVector3 BC = (*triPoints[2]) - (*triPoints[1]); CCVector3 CA = (*triPoints[0]) - (*triPoints[2]); //be sure that the triangle is not degenerate!!! if (AB.norm2() > ZERO_TOLERANCE && BC.norm2() > ZERO_TOLERANCE && CA.norm2() > ZERO_TOLERANCE) { Tuple3i cellPos[3]; { for (int k = 0; k < 3; k++) { CCVector3 P = *(triPoints[k]) - gridMinCorner; cellPos[k].x = std::min(static_cast<int>(P.x / cellLength), static_cast<int>(size().x) - 1); cellPos[k].y = std::min(static_cast<int>(P.y / cellLength), static_cast<int>(size().y) - 1); cellPos[k].z = std::min(static_cast<int>(P.z / cellLength), static_cast<int>(size().z) - 1); } } //compute the triangle bounding-box Tuple3i minPos, maxPos; { for (int k = 0; k < 3; k++) { minPos.u[k] = std::min(cellPos[0].u[k], std::min(cellPos[1].u[k], cellPos[2].u[k])); maxPos.u[k] = std::max(cellPos[0].u[k], std::max(cellPos[1].u[k], cellPos[2].u[k])); } } //first cell assert(cellsToTest.capacity() != 0); cellsToTestCount = 1; CellToTest* _currentCell = &cellsToTest[0/*cellsToTestCount-1*/]; _currentCell->pos = minPos; //max distance (in terms of cell) between the vertices int maxSize = 0; { Tuple3i delta = maxPos - minPos + Tuple3i(1, 1, 1); maxSize = std::max(delta.x, delta.y); maxSize = std::max(maxSize, delta.z); } //test each cell for (int k = minPos.z; k <= maxPos.z; ++k) { CCVector3 C(0, 0, gridMinCorner.z + k * cellLength + halfCellSize); for (int j = minPos.y; j <= maxPos.y; ++j) { C.y = gridMinCorner.y + j * cellLength + halfCellSize; for (int i = minPos.x; i <= maxPos.x; ++i) { //compute the (absolute) cell center C.x = gridMinCorner.x + i * cellLength + halfCellSize; ScalarType dist = DistanceComputationTools::computePoint2TriangleDistance(&C, T, true); GridElement& cellValue = getValue(i, j, k); if (fabs(cellValue) > fabs(dist)) { cellValue = dist; } } } } } if (progressCb && !nProgress.oneStep()) { //cancel by user return false; } } return true; }
ccGLMatrix ccGLMatrix::FromToRotation(const CCVector3& from, const CCVector3& to) { float e = from.dot(to); float f = (e < 0 ? -e : e); ccGLMatrix result; float* mat = result.data(); if (f > 1.0-ZERO_TOLERANCE) //"from" and "to"-vector almost parallel { CCVector3 x; // vector most nearly orthogonal to "from" x.x = (from.x > 0 ? from.x : -from.x); x.y = (from.y > 0 ? from.y : -from.y); x.z = (from.z > 0 ? from.z : -from.z); if (x.x < x.y) { if (x.x < x.z) { x.x = 1.0f; x.y = x.z = 0; } else { x.z = 1.0f; x.x = x.y = 0; } } else { if (x.y < x.z) { x.y = 1.0f; x.x = x.z = 0; } else { x.z = 1.0f; x.x = x.y = 0; } } CCVector3 u(x.x-from.x, x.y-from.y, x.z-from.z); CCVector3 v(x.x-to.x, x.y-to.y, x.z-to.z); float c1 = 2.0f / u.dot(u); float c2 = 2.0f / v.dot(v); float c3 = c1 * c2 * u.dot(v); for (unsigned i = 0; i < 3; i++) { for (unsigned j = 0; j < 3; j++) { mat[i*4+j]= c3 * v.u[i] * u.u[j] - c2 * v.u[i] * v.u[j] - c1 * u.u[i] * u.u[j]; } mat[i*4+i] += 1.0f; } } else // the most common case, unless "from"="to", or "from"=-"to" { //hand optimized version (9 mults less) CCVector3 v = from.cross(to); float h = 1.0f/(1.0f + e); float hvx = h * v.x; float hvz = h * v.z; float hvxy = hvx * v.y; float hvxz = hvx * v.z; float hvyz = hvz * v.y; mat[0] = e + hvx * v.x; mat[1] = hvxy - v.z; mat[2] = hvxz + v.y; mat[4] = hvxy + v.z; mat[5] = e + h * v.y * v.y; mat[6] = hvyz - v.x; mat[8] = hvxz - v.y; mat[9] = hvyz + v.x; mat[10] = e + hvz * v.z; } return result; }