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;
}
