bool ccClipBox::move2D(int x, int y, int dx, int dy, int screenWidth, int screenHeight)
{
if (m_activeComponent != SPHERE || !m_box.isValid())
return false;
//convert mouse position to vector (screen-centered)
CCVector3d currentOrientation = PointToVector(x,y,screenWidth,screenHeight);
ccGLMatrixd rotMat = ccGLMatrixd::FromToRotation(m_lastOrientation,currentOrientation);
CCVector3 C = m_box.getCenter();
ccGLMatrixd transMat;
transMat.setTranslation(-C);
transMat = rotMat * transMat;
transMat.setTranslation(transMat.getTranslationAsVec3D() + CCVector3d::fromArray(C.u));
//rotateGL(transMat);
m_glTrans = ccGLMatrix(transMat.inverse().data()) * m_glTrans;
enableGLTransformation(true);
m_lastOrientation = currentOrientation;
update();
return true;
}
bool ccClipBox::move3D(const CCVector3d& uInput)
{
if (m_activeComponent == NONE || !m_box.isValid())
return false;
CCVector3d u = uInput;
//Arrows
if (m_activeComponent >= X_MINUS_ARROW && m_activeComponent <= CROSS)
{
if (m_glTransEnabled)
m_glTrans.inverse().applyRotation(u);
switch(m_activeComponent)
{
case X_MINUS_ARROW:
m_box.minCorner().x += static_cast<PointCoordinateType>(u.x);
if (m_box.minCorner().x > m_box.maxCorner().x)
m_box.minCorner().x = m_box.maxCorner().x;
break;
case X_PLUS_ARROW:
m_box.maxCorner().x += static_cast<PointCoordinateType>(u.x);
if (m_box.minCorner().x > m_box.maxCorner().x)
m_box.maxCorner().x = m_box.minCorner().x;
break;
case Y_MINUS_ARROW:
m_box.minCorner().y += static_cast<PointCoordinateType>(u.y);
if (m_box.minCorner().y > m_box.maxCorner().y)
m_box.minCorner().y = m_box.maxCorner().y;
break;
case Y_PLUS_ARROW:
m_box.maxCorner().y += static_cast<PointCoordinateType>(u.y);
if (m_box.minCorner().y > m_box.maxCorner().y)
m_box.maxCorner().y = m_box.minCorner().y;
break;
case Z_MINUS_ARROW:
m_box.minCorner().z += static_cast<PointCoordinateType>(u.z);
if (m_box.minCorner().z > m_box.maxCorner().z)
m_box.minCorner().z = m_box.maxCorner().z;
break;
case Z_PLUS_ARROW:
m_box.maxCorner().z += static_cast<PointCoordinateType>(u.z);
if (m_box.minCorner().z > m_box.maxCorner().z)
m_box.maxCorner().z = m_box.minCorner().z;
break;
case CROSS:
m_box += CCVector3::fromArray(u.u);
break;
default:
assert(false);
return false;
}
//send 'modified' signal
emit boxModified(&m_box);
}
else if (m_activeComponent == SPHERE)
{
//handled by move2D!
return false;
}
else if (m_activeComponent >= X_MINUS_TORUS && m_activeComponent <= Z_PLUS_TORUS)
{
//we guess the rotation order by comparing the current screen 'normal'
//and the vector prod of u and the current rotation axis
CCVector3d Rb(0,0,0);
switch(m_activeComponent)
{
case X_MINUS_TORUS:
Rb.x = -1;
break;
case X_PLUS_TORUS:
Rb.x = 1;
break;
case Y_MINUS_TORUS:
Rb.y = -1;
break;
case Y_PLUS_TORUS:
Rb.y = 1;
break;
case Z_MINUS_TORUS:
Rb.z = -1;
break;
case Z_PLUS_TORUS:
Rb.z = 1;
break;
default:
assert(false);
return false;
}
CCVector3d R = Rb;
if (m_glTransEnabled)
m_glTrans.applyRotation(R);
CCVector3d RxU = R.cross(u);
//look for the most parallel dimension
int minDim = 0;
double maxDot = m_viewMatrix.getColumnAsVec3D(0).dot(RxU);
for (int i=1; i<3; ++i)
{
double dot = m_viewMatrix.getColumnAsVec3D(i).dot(RxU);
if (fabs(dot) > fabs(maxDot))
{
maxDot = dot;
minDim = i;
}
}
//angle is proportional to absolute displacement
double angle_rad = u.norm()/m_box.getDiagNorm() * M_PI;
if (maxDot < 0.0)
angle_rad = -angle_rad;
ccGLMatrixd rotMat;
rotMat.initFromParameters(angle_rad,Rb,CCVector3d(0,0,0));
CCVector3 C = m_box.getCenter();
ccGLMatrixd transMat;
transMat.setTranslation(-C);
transMat = rotMat * transMat;
transMat.setTranslation(transMat.getTranslationAsVec3D() + CCVector3d::fromArray(C.u));
m_glTrans = m_glTrans * ccGLMatrix(transMat.inverse().data());
enableGLTransformation(true);
}
else
{
assert(false);
return false;
}
update();
return true;
}
ccGLMatrix::ccGLMatrix(const CCLib::SquareMatrix& R, const CCVector3& T, const CCVector3& rotCenter)
{
*this = ccGLMatrix(R,T);
shiftRotationCenter(rotCenter);
}
bool ccRegistrationTools::ICP( ccHObject* data,
ccHObject* model,
ccGLMatrix& transMat,
double &finalScale,
double& finalError,
double minErrorDecrease,
unsigned maxIterationCount,
unsigned randomSamplingLimit,
bool removeFarthestPoints,
ConvergenceMethod method,
bool adjustScale,
bool useDataSFAsWeights/*=false*/,
bool useModelSFAsWeights/*=false*/,
QWidget* parent/*=0*/)
{
//progress bar
ccProgressDialog pDlg(false,parent);
//if the 'model' entity is a mesh, we need to sample points on it
CCLib::GenericIndexedCloudPersist* modelCloud = 0;
if (model->isKindOf(CC_TYPES::MESH))
{
modelCloud = CCLib::MeshSamplingTools::samplePointsOnMesh(ccHObjectCaster::ToGenericMesh(model),s_defaultSampledPointsOnModelMesh,&pDlg);
if (!modelCloud)
{
ccLog::Error("[ICP] Failed to sample points on 'model' mesh!");
return false;
}
}
else
{
modelCloud = ccHObjectCaster::ToGenericPointCloud(model);
}
//if the 'data' entity is a mesh, we need to sample points on it
CCLib::GenericIndexedCloudPersist* dataCloud = 0;
if (data->isKindOf(CC_TYPES::MESH))
{
dataCloud = CCLib::MeshSamplingTools::samplePointsOnMesh(ccHObjectCaster::ToGenericMesh(data),s_defaultSampledPointsOnDataMesh,&pDlg);
if (!dataCloud)
{
ccLog::Error("[ICP] Failed to sample points on 'data' mesh!");
return false;
}
}
else
{
dataCloud = ccHObjectCaster::ToGenericPointCloud(data);
}
//we activate a temporary scalar field for registration distances computation
CCLib::ScalarField* dataDisplayedSF = 0;
int oldDataSfIdx=-1, dataSfIdx=-1;
//if the 'data' entity is a real ccPointCloud, we can even create a temporary SF for registration distances
if (data->isA(CC_TYPES::POINT_CLOUD))
{
ccPointCloud* pc = static_cast<ccPointCloud*>(data);
dataDisplayedSF = pc->getCurrentDisplayedScalarField();
oldDataSfIdx = pc->getCurrentInScalarFieldIndex();
dataSfIdx = pc->getScalarFieldIndexByName(REGISTRATION_DISTS_SF);
if (dataSfIdx < 0)
dataSfIdx = pc->addScalarField(REGISTRATION_DISTS_SF);
if (dataSfIdx >= 0)
pc->setCurrentScalarField(dataSfIdx);
else
ccLog::Warning("[ICP] Couldn't create temporary scalar field! Not enough memory?");
}
else
{
dataCloud->enableScalarField();
}
//parameters
CCLib::PointProjectionTools::Transformation transform;
CCLib::ScalarField* modelWeights = 0;
if (useModelSFAsWeights)
{
if (modelCloud == dynamic_cast<CCLib::GenericIndexedCloudPersist*>(model) && model->isA(CC_TYPES::POINT_CLOUD))
{
ccPointCloud* pc = static_cast<ccPointCloud*>(model);
modelWeights = pc->getCurrentDisplayedScalarField();
if (!modelWeights)
ccLog::Warning("[ICP] 'useDataSFAsWeights' is true but model has no displayed scalar field!");
}
else
{
ccLog::Warning("[ICP] 'useDataSFAsWeights' is true but only point clouds scalar fields can be used as weights!");
}
}
CCLib::ScalarField* dataWeights = 0;
if (useDataSFAsWeights)
{
if (!dataDisplayedSF)
{
if (dataCloud == (CCLib::GenericIndexedCloudPersist*)data && data->isA(CC_TYPES::POINT_CLOUD))
ccLog::Warning("[ICP] 'useDataSFAsWeights' is true but data has no displayed scalar field!");
else
ccLog::Warning("[ICP] 'useDataSFAsWeights' is true but inly point clouds scalar fields can be used as weights!");
}
else
dataWeights = dataDisplayedSF;
}
CCLib::ICPRegistrationTools::CC_ICP_RESULT result;
result = CCLib::ICPRegistrationTools::RegisterClouds( modelCloud,
dataCloud,
transform,
method,
minErrorDecrease,
maxIterationCount,
finalError,
adjustScale,
static_cast<CCLib::GenericProgressCallback*>(&pDlg),
removeFarthestPoints,
randomSamplingLimit,
modelWeights,
dataWeights);
if (result >= CCLib::ICPRegistrationTools::ICP_ERROR)
{
ccLog::Error("Registration failed: an error occurred (code %i)",result);
}
else if (result == CCLib::ICPRegistrationTools::ICP_APPLY_TRANSFO)
{
transMat = ccGLMatrix(transform.R, transform.T, transform.s);
finalScale = transform.s;
}
//if we had to sample points an the data mesh
if (!data->isKindOf(CC_TYPES::POINT_CLOUD))
{
delete dataCloud;
dataCloud = 0;
}
else
{
if (data->isA(CC_TYPES::POINT_CLOUD))
{
ccPointCloud* pc = static_cast<ccPointCloud*>(data);
pc->setCurrentScalarField(oldDataSfIdx);
if (dataSfIdx >= 0)
pc->deleteScalarField(dataSfIdx);
dataSfIdx=-1;
}
}
//if we had to sample points an the model mesh
if (!model->isKindOf(CC_TYPES::POINT_CLOUD))
{
delete modelCloud;
modelCloud = 0;
}
return (result < CCLib::ICPRegistrationTools::ICP_ERROR);
}
