void TSShapeInstance::handleAnimatedScale(TSThread * thread, S32 a, S32 b, TSIntegerSet & scaleBeenSet)
{
S32 j=0;
S32 start = thread->getSequence()->scaleMatters.start();
S32 end = b;
// code the scale conversion (might need to "upgrade" from uniform to arbitrary, e.g.)
// code uniform, aligned, and arbitrary as 0,1, and 2, respectively,
// with sequence coding in first two bits, shape coding in next two bits
S32 code = 0;
if (thread->getSequence()->animatesAlignedScale())
code += 1;
else if (thread->getSequence()->animatesArbitraryScale())
code += 2;
if (animatesAlignedScale())
code += 4;
if (animatesArbitraryScale())
code += 8;
F32 uniformScale = 1.0f;
Point3F alignedScale(0.0f, 0.0f, 0.0f);
TSScale arbitraryScale;
for (S32 nodeIndex=start; nodeIndex<end; thread->getSequence()->scaleMatters.next(nodeIndex), j++)
{
if (nodeIndex<a)
continue;
if (!scaleBeenSet.test(nodeIndex))
{
// compute scale in sequence format
switch (code)
{ // Sequence Shape
case 0: // uniform -> uniform
case 4: // uniform -> aligned
case 8: // uniform -> arbitrary
{
F32 s1 = mShape->getUniformScale(*thread->getSequence(),thread->keyNum1,j);
F32 s2 = mShape->getUniformScale(*thread->getSequence(),thread->keyNum2,j);
uniformScale = TSTransform::interpolate(s1,s2,thread->keyPos);
alignedScale.set(uniformScale,uniformScale,uniformScale);
break;
}
case 5: // aligned -> aligned
case 9: // aligned -> arbitrary
{
const Point3F & s1 = mShape->getAlignedScale(*thread->getSequence(),thread->keyNum1,j);
const Point3F & s2 = mShape->getAlignedScale(*thread->getSequence(),thread->keyNum2,j);
TSTransform::interpolate(s1,s2,thread->keyPos,&alignedScale);
break;
}
case 10: // arbitrary -> arbitary
{
TSScale s1,s2;
mShape->getArbitraryScale(*thread->getSequence(),thread->keyNum1,j,&s1);
mShape->getArbitraryScale(*thread->getSequence(),thread->keyNum2,j,&s2);
TSTransform::interpolate(s1,s2,thread->keyPos,&arbitraryScale);
break;
}
default: AssertFatal(0,"TSShapeInstance::handleAnimatedScale"); break;
}
switch (code)
{
case 0: // uniform -> uniform
{
mCurrentRenderState->smNodeCurrentUniformScales[nodeIndex] = uniformScale;
break;
}
case 4: // uniform -> aligned
case 5: // aligned -> aligned
mCurrentRenderState->smNodeCurrentAlignedScales[nodeIndex] = alignedScale;
break;
case 8: // uniform -> arbitrary
case 9: // aligned -> arbitrary
{
mCurrentRenderState->smNodeCurrentArbitraryScales[nodeIndex].identity();
mCurrentRenderState->smNodeCurrentArbitraryScales[nodeIndex].mScale = alignedScale;
break;
}
case 10: // arbitrary -> arbitary
{
mCurrentRenderState->smNodeCurrentArbitraryScales[nodeIndex] = arbitraryScale;
break;
}
default: AssertFatal(0,"TSShapeInstance::handleAnimatedScale"); break;
}
mCurrentRenderState->smScaleThreads[nodeIndex] = thread;
scaleBeenSet.set(nodeIndex);
}
}
}
unsigned int CObjTreePlugin::insertBBox(unsigned int id, const geometry_msgs::Pose &pose, const geometry_msgs::Vector3 &scale, CObjTreePlugin::Operation op)
{
printf("insertBoundingBox called, mode %d\n", op);
if(op == INSERT && m_octree.removeObject(id))
{
//Updating existing box
removePrimitiveMarker(id);
}
objtree::Point newPosition(pose.position.x, pose.position.y, pose.position.z);
objtree::Vector4f newOrientation(pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w);
objtree::Point newScale(scale.x, scale.y, scale.z);
//Compute minimal aligned bounding box
Eigen::Vector3f min, max;
Eigen::Vector3f vec[4];
vec[0] = Eigen::Vector3f(-scale.x/2.0f, -scale.y/2.0f, -scale.z/2.0f);
vec[1] = Eigen::Vector3f(-scale.x/2.0f, scale.y/2.0f, -scale.z/2.0f);
vec[2] = Eigen::Vector3f( scale.x/2.0f, -scale.y/2.0f, -scale.z/2.0f);
vec[3] = Eigen::Vector3f( scale.x/2.0f, scale.y/2.0f, -scale.z/2.0f);
Eigen::Quaternionf q(pose.orientation.w, pose.orientation.x, pose.orientation.y, pose.orientation.z);
min = max = q*vec[0];
for(int i = 1; i < 4; i++)
{
vec[i] = q*vec[i];
for(int j = 0; j < 3; j++)
{
if(min[j] > vec[i][j]) min[j] = vec[i][j];
if(max[j] < vec[i][j]) max[j] = vec[i][j];
}
}
for(int i = 0; i < 4; i++)
{
vec[i] = -vec[i];
for(int j = 0; j < 3; j++)
{
if(min[j] > vec[i][j]) min[j] = vec[i][j];
if(max[j] < vec[i][j]) max[j] = vec[i][j];
}
}
Eigen::Vector3f alignedScale(max-min);
objtree::Box alignedBox(min[0]+newPosition.x, min[1]+newPosition.y, min[2]+newPosition.z, alignedScale[0], alignedScale[1], alignedScale[2]);
objtree::GBBox *newBox = new objtree::GBBox(newPosition, newOrientation, newScale, alignedBox);
newBox->setId(id);
switch(op)
{
case INSERT: return m_octree.insert(newBox);
case UPDATE: return m_octree.insertUpdate(newBox);
case GET_SIMILAR:
{
unsigned int id = -1;
const objtree::Object *object = m_octree.getSimilarObject(newBox);
if(object)
{
id = object->id();
}
delete newBox;
return id;
}
}
return -1;
}
