void testPointPlaneICP()
{
Random rnd( 0 );
Matrix4f dstToSrc = Matrix4f::translation( 0.1f, -0.42f, 0.2f ) * Matrix4f::rotateX( MathUtils::degreesToRadians( 10.f ) );
//Matrix4f dstToSrc = Matrix4f::rotateX( MathUtils::degreesToRadians( 10.f ) );
//Matrix4f dstToSrc = Matrix4f::rotateX( MathUtils::degreesToRadians( 20.f ) );
Matrix4f srcToDstGT = dstToSrc.inverse();
int nPoints = 1024;
std::vector< Vector3f > srcPoints( nPoints );
std::vector< Vector3f > dstPoints( nPoints );
std::vector< Vector3f > dstNormals( nPoints );
for( size_t i = 0; i < dstPoints.size(); ++i )
{
dstPoints[i] = Vector3f( rnd.nextFloat(), rnd.nextFloat(), rnd.nextFloat() );
dstNormals[i] = Sampling::areaSampleSphere( rnd.nextFloat(), rnd.nextFloat() );
srcPoints[i] = dstToSrc.transformPoint( dstPoints[i] );
}
PointPlaneICP icp( 6, 0.01f );
Matrix4f initialGuess = Matrix4f::identity();
Matrix4f mSolution;
bool succeeded = icp.align( srcPoints, dstPoints, dstNormals, initialGuess, mSolution );
Matrix4f diff = srcToDstGT.inverse() - mSolution;
diff.print();
}
void AnimeshObject::Skin ()
{
if (!skeleton)
return;
CS_ASSERT (SkinV ?
skinnedVertices->GetElementCount () >= factory->vertexCount : true);
CS_ASSERT (SkinN ?
skinnedNormals->GetElementCount () >= factory->vertexCount : true);
CS_ASSERT (SkinTB ?
skinnedTangents->GetElementCount () >= factory->vertexCount
&& skinnedBinormals->GetElementCount () >= factory->vertexCount
: true);
// Setup some local data
csVertexListWalker<float, csVector3> srcVerts (postMorphVertices);
csRenderBufferLock<csVector3> dstVerts (skinnedVertices);
csVertexListWalker<float, csVector3> srcNormals (factory->normalBuffer);
csRenderBufferLock<csVector3> dstNormals (skinnedNormals);
csVertexListWalker<float, csVector3> srcTangents (factory->tangentBuffer);
csRenderBufferLock<csVector3> dstTangents (skinnedTangents);
csVertexListWalker<float, csVector3> srcBinormals (factory->binormalBuffer);
csRenderBufferLock<csVector3> dstBinormals (skinnedBinormals);
csSkeletalState2* skeletonState = lastSkeletonState;
csAnimatedMeshBoneInfluence* influence = factory->boneInfluences.GetArray ();
for (size_t i = 0; i < factory->vertexCount; ++i)
{
// Accumulate data for the vertex
int numInfluences = 0;
csDualQuaternion dq (csQuaternion (0,0,0,0), csQuaternion (0,0,0,0));
csQuaternion pivot;
for (size_t j = 0; j < 4; ++j, ++influence) // @@SOLVE 4
{
if (influence->influenceWeight > 0.0f)
{
numInfluences++;
csDualQuaternion inflQuat (
skeletonState->GetQuaternion (influence->bone),
skeletonState->GetVector (influence->bone));
if (numInfluences == 1)
{
pivot = inflQuat.real;
}
else if (inflQuat.real.Dot (pivot) < 0.0f)
{
inflQuat *= -1.0f;
}
dq += inflQuat * influence->influenceWeight;
}
}
if (numInfluences == 0)
{
if (SkinV)
{
dstVerts[i] = *srcVerts;
}
if (SkinN)
{
dstNormals[i] = *srcNormals;
}
if (SkinTB)
{
dstTangents[i] = *srcTangents;
dstBinormals[i] = *srcBinormals;
}
}
else
{
dq = dq.Unit ();
if (SkinV)
{
dstVerts[i] = dq.TransformPoint (*srcVerts);
}
if (SkinN)
{
dstNormals[i] = dq.Transform (*srcNormals);
}
if (SkinTB)
{
dstTangents[i] = dq.Transform (*srcTangents);
dstBinormals[i] = dq.Transform (*srcBinormals);
}
}
++srcVerts;
++srcNormals;
++srcTangents;
++srcBinormals;
}
}
