void CalSpringSystem::calculateVertices(CalSubmesh *pSubmesh, float deltaTime)
{
// get the vertex vector of the submesh
std::vector<CalVector>& vectorVertex = pSubmesh->getVectorVertex();
// get the physical property vector of the submesh
std::vector<CalSubmesh::PhysicalProperty>& vectorPhysicalProperty = pSubmesh->getVectorPhysicalProperty();
// get the physical property vector of the core submesh
std::vector<CalCoreSubmesh::PhysicalProperty>& vectorCorePhysicalProperty = pSubmesh->getCoreSubmesh()->getVectorPhysicalProperty();
// loop through all the vertices
int vertexId;
for(vertexId = 0; vertexId < (int)vectorVertex.size(); ++vertexId)
{
// get the vertex
CalVector& vertex = vectorVertex[vertexId];
// get the physical property of the vertex
CalSubmesh::PhysicalProperty& physicalProperty = vectorPhysicalProperty[vertexId];
// get the physical property of the core vertex
CalCoreSubmesh::PhysicalProperty& corePhysicalProperty = vectorCorePhysicalProperty[vertexId];
// store current position for later use
CalVector position;
position = physicalProperty.position;
// only take vertices with a weight > 0 into account
if(corePhysicalProperty.weight > 0.0f)
{
// do the Verlet step
physicalProperty.position += (position - physicalProperty.positionOld) * 0.99f + physicalProperty.force / corePhysicalProperty.weight * deltaTime * deltaTime;
CalSkeleton *pSkeleton = m_pModel->getSkeleton();
if(m_collision)
{
std::vector<CalBone *> &m_vectorbone = pSkeleton->getVectorBone();
unsigned long boneId;
for(boneId=0; boneId < m_vectorbone.size(); boneId++)
{
CalBoundingBox p = m_vectorbone[boneId]->getBoundingBox();
bool in=true;
float min=1e10;
int index=-1;
int faceId;
for(faceId=0; faceId < 6 ; faceId++)
{
if(p.plane[faceId].eval(physicalProperty.position)<=0)
{
in=false;
}
else
{
float dist=p.plane[faceId].dist(physicalProperty.position);
if(dist<min)
{
index=faceId;
min=dist;
}
}
}
if(in && index!=-1)
{
CalVector normal = CalVector(p.plane[index].a,p.plane[index].b,normal.z = p.plane[index].c);
normal.normalize();
physicalProperty.position = physicalProperty.position - min*normal;
}
in=true;
for(faceId=0; faceId < 6 ; faceId++)
{
if(p.plane[faceId].eval(physicalProperty.position) < 0 )
{
in=false;
}
}
if(in)
{
physicalProperty.position = vectorVertex[vertexId];
}
}
}
}
else
{
physicalProperty.position = vectorVertex[vertexId];
}
// make the current position the old one
physicalProperty.positionOld = position;
// set the new position of the vertex
vertex = physicalProperty.position;
// clear the accumulated force on the vertex
physicalProperty.force.clear();
}
// get the spring vector of the core submesh
std::vector<CalCoreSubmesh::Spring>& vectorSpring = pSubmesh->getCoreSubmesh()->getVectorSpring();
// iterate a few times to relax the constraints
int iterationCount;
#define ITERATION_COUNT 2
for(iterationCount = 0; iterationCount < ITERATION_COUNT; ++iterationCount)
{
// loop through all the springs
std::vector<CalCoreSubmesh::Spring>::iterator iteratorSpring;
for(iteratorSpring = vectorSpring.begin(); iteratorSpring != vectorSpring.end(); ++iteratorSpring)
{
// get the spring
CalCoreSubmesh::Spring& spring = *iteratorSpring;
// compute the difference between the two spring vertices
CalVector distance;
distance = vectorVertex[spring.vertexId[1]] - vectorVertex[spring.vertexId[0]];
// get the current length of the spring
float length;
length = distance.length();
if(length > 0.0f)
{
/*if (spring.springCoefficient == 0)
{
vectorVertex[spring.vertexId[1]] = vectorVertex[spring.vertexId[0]];
vectorPhysicalProperty[spring.vertexId[1]].position = vectorVertex[spring.vertexId[0]];
}
else
{*/
float factor[2];
factor[0] = (length - spring.idleLength) / length;
factor[1] = factor[0];
if(vectorCorePhysicalProperty[spring.vertexId[0]].weight > 0.0f)
{
factor[0] /= 2.0f;
factor[1] /= 2.0f;
}
else
{
factor[0] = 0.0f;
}
if(vectorCorePhysicalProperty[spring.vertexId[1]].weight <= 0.0f)
{
factor[0] *= 2.0f;
factor[1] = 0.0f;
}
vectorVertex[spring.vertexId[0]] += distance * factor[0];
vectorPhysicalProperty[spring.vertexId[0]].position = vectorVertex[spring.vertexId[0]];
vectorVertex[spring.vertexId[1]] -= distance * factor[1];
vectorPhysicalProperty[spring.vertexId[1]].position = vectorVertex[spring.vertexId[1]];
//}
}
}
}
/* DEBUG-CODE ********************
CalVector spherePosition(Sphere.x, Sphere.y, Sphere.z);
float sphereRadius = Sphere.radius;
// loop through all the vertices
for(vertexId = 0; vertexId < (int)vectorVertex.size(); ++vertexId)
{
// get the vertex
CalVector& vertex = vectorVertex[vertexId];
// get the physical property of the vertex
CalSubmesh::PhysicalProperty& physicalProperty = vectorPhysicalProperty[vertexId];
// get the physical property of the core vertex
CalCoreSubmesh::PhysicalProperty& corePhysicalProperty = vectorCorePhysicalProperty[vertexId];
// only take vertices with a weight > 0 into account
if(corePhysicalProperty.weight > 0.0f)
{
CalVector position;
position = physicalProperty.position;
position -= spherePosition;
float length;
length = position.normalize();
if(length < sphereRadius)
{
position *= sphereRadius;
position += spherePosition;
physicalProperty.position = position;
physicalProperty.positionOld = position;
vertex = physicalProperty.position;
}
}
}
*********************************/
}
static
void
generateTangentAndHandednessBuffer( osgCal::MeshData* m,
const CalIndex* indexBuffer )
{
if ( !m->texCoordBuffer.valid() )
{
return;
}
int vertexCount = m->vertexBuffer->size();
int faceCount = m->getIndicesCount() / 3;
m->tangentAndHandednessBuffer = new TangentAndHandednessBuffer( vertexCount );
CalVector* tan1 = new CalVector[vertexCount];
CalVector* tan2 = new CalVector[vertexCount];
const GLfloat* texCoordBufferData = (GLfloat*) m->texCoordBuffer->getDataPointer();
const GLfloat* vb = (GLfloat*) m->vertexBuffer->getDataPointer();
#ifdef OSG_CAL_BYTE_BUFFERS
GLfloat* thb = new GLfloat[ vertexCount*4 ];
const GLfloat* nb = floatNormalBuffer;
#else
GLfloat* thb = (GLfloat*) m->tangentAndHandednessBuffer->getDataPointer();
// GLshort* thb = (GLshort*) m->tangentAndHandednessBuffer->getDataPointer();
const GLfloat* nb = (GLfloat*) m->normalBuffer->getDataPointer();
#endif
for ( int face = 0; face < faceCount; face++ )
{
for ( int j = 0; j < 3; j++ )
{
// there seems to be no visual difference in calculating
// tangent per vertex (as is tan1[i1] += spos(j=0,1,2))
// or per face (tan1[i1,i2,i3] += spos)
CalIndex i1 = indexBuffer[face*3+(j+0)%3];
CalIndex i2 = indexBuffer[face*3+(j+1)%3];
CalIndex i3 = indexBuffer[face*3+(j+2)%3];
const float* v1 = &vb[i1*3];
const float* v2 = &vb[i2*3];
const float* v3 = &vb[i3*3];
const float* w1 = &texCoordBufferData[i1*2];
const float* w2 = &texCoordBufferData[i2*2];
const float* w3 = &texCoordBufferData[i3*2];
#define x(_a) (_a[0])
#define y(_a) (_a[1])
#define z(_a) (_a[2])
float x1 = x(v2) - x(v1);
float x2 = x(v3) - x(v1);
float y1 = y(v2) - y(v1);
float y2 = y(v3) - y(v1);
float z1 = z(v2) - z(v1);
float z2 = z(v3) - z(v1);
float s1 = x(w2) - x(w1);
float s2 = x(w3) - x(w1);
float t1 = y(w2) - y(w1);
float t2 = y(w3) - y(w1);
#undef x
#undef y
#undef z
//float r = 1.0F / (s1 * t2 - s2 * t1);
float r = (s1 * t2 - s2 * t1) < 0 ? -1.0 : 1.0;
CalVector sdir((t2 * x1 - t1 * x2) * r, (t2 * y1 - t1 * y2) * r,
(t2 * z1 - t1 * z2) * r);
CalVector tdir((s1 * x2 - s2 * x1) * r, (s1 * y2 - s2 * y1) * r,
(s1 * z2 - s2 * z1) * r);
// sdir & tdir can be 0 (when UV unwrap doesn't exists
// or has errors like coincide points)
// we ignore them
if ( sdir.length() > 0 )
{
sdir.normalize();
tan1[i1] += sdir;
//tan1[i2] += sdir;
//tan1[i3] += sdir;
}
if ( tdir.length() > 0 )
{
tdir.normalize();
tan2[i1] += tdir;
//tan2[i2] += tdir;
//tan2[i3] += tdir;
}
}
}
for (long a = 0; a < vertexCount; a++)
{
CalVector tangent;
CalVector binormal;
CalVector t = tan1[a];
CalVector b = tan2[a];
CalVector n = CalVector( nb[a*3+0],
nb[a*3+1],
nb[a*3+2] );
// tangent & bitangent can be zero when UV unwrap doesn't exists
// or has errors like coincide points
if ( t.length() > 0 )
{
t.normalize();
// Gram-Schmidt orthogonalize
tangent = t - n * (n*t);
tangent.normalize();
// Calculate handedness
binormal = CalVector(n % tangent) *
((((n % t) * b) < 0.0F) ? -1.0f : 1.0f);
binormal.normalize();
}
else if ( b.length() > 0 )
{
b.normalize();
// Gram-Schmidt orthogonalize
binormal = b - n * (n*b);
binormal.normalize();
// Calculate handedness
tangent = CalVector(n % binormal) *
((((n % b) * t) < 0.0F) ? -1.0f : 1.0f);
tangent.normalize();
}
// std::cout << "t = " << tangent.x << '\t' << tangent.y << '\t' << tangent.z << '\n';
// std::cout << "b = " << binormal.x << '\t' << binormal.y << '\t' << binormal.z << '\n';
// std::cout << "n = " << n.x << '\t' << n.y << '\t' << n.z << '\n';
// thb[a*4+0] = floatToHalf( tangent.x ); //tangent.x * 0x7FFF;
// thb[a*4+1] = floatToHalf( tangent.y ); //tangent.y * 0x7FFF;
// thb[a*4+2] = floatToHalf( tangent.z ); //tangent.z * 0x7FFF;
// thb[a*4+3] = floatToHalf((((n % tangent) * binormal) > 0.0F) ? -1.0f : 1.0f); // handedness
thb[a*4+0] = tangent.x;
thb[a*4+1] = tangent.y;
thb[a*4+2] = tangent.z;
thb[a*4+3] = ((((n % tangent) * binormal) > 0.0F) ? -1.0f : 1.0f); // handedness
}
delete[] tan1;
delete[] tan2;
#ifdef OSG_CAL_BYTE_BUFFERS
GLbyte* tangents = (GLbyte*) tangentBuffer->getDataPointer();
GLbyte* binormals = (GLbyte*) binormalBuffer->getDataPointer();
for ( int i = 0; i < vertexCount*3; i++ )
{
tangents[i] = static_cast< GLbyte >( tangentBuffer[i]*127.0 );
binormals[i] = static_cast< GLbyte >( binormalBuffer[i]*127.0 );
//std::cout << (int)tangents[i] << '\n';
}
delete[] tangentBuffer;
delete[] binormalBuffer;
#endif
}
void IKCharacter::solve( int iterations, int root_id, int leaf_id )
{
while ( iterations>0 )
{
// start at leaf nodes
deque<int> queue;
// push leaf bones to target positions
for ( int i=0; i<leaf_bones.size(); i++ )
{
// if we have a leaf id to use, skip all other leaves
if ( leaf_id != -1 && leaf_id != leaf_bones[i] )
continue;
// if we have a target for this one
if ( leaf_targets.find(leaf_bones[i]) != leaf_targets.end() )
{
// set it
world_positions[leaf_bones[i]] = leaf_targets[leaf_bones[i]].second;
}
int parent_id = skeleton->getCoreSkeleton()->getCoreBone( leaf_bones[i] )->getParentId();
if ( parent_id != -1 )
queue.push_back( parent_id );
}
// queue to handle branching
deque< int > branch_queue;
while ( !queue.empty() || !branch_queue.empty() )
{
// if main queue is empty then we are ready to deal with branches
if ( queue.empty() )
{
queue.push_back( branch_queue.front() );
branch_queue.pop_front();
continue;
}
int next_id = queue.front();
queue.pop_front();
// bail out if we should
if ( root_id != -1 && next_id == root_id )
continue;
CalBone* bone = skeleton->getBone( next_id );
list<int>& children = bone->getCoreBone()->getListChildId();
// is this a branch?
if ( children.size()>1 )
{
// still other children to process -- push to branch queue
if ( !queue.empty() )
{
branch_queue.push_back( next_id );
continue;
}
// otherwise, process branch here
// if we're here, then all the children of this branch have been processed already
int parent_id = bone->getCoreBone()->getParentId();
if ( parent_id != -1 )
{
//printf("averaging %lu positions for %s wc %f: ", children.size(), bone->getCoreBone()->getName().c_str(), getWeightCentre( next_id ) );
// fetch all child positions
vector<CalVector> results;
results.insert( results.begin(), children.size(), world_positions[next_id] );
int count=0;
for ( list<int>::iterator it = children.begin(); it != children.end(); ++it,++count )
{
// child_pos
CalVector& b_c = world_positions[*it];
// current pos
CalVector& b_p = results[count];
// now, the bone is the wrong length. correct its length to fulfil size constraint.
CalVector delta = b_c - b_p;
float length = delta.length();
length = max( 0.00001f, length );
// pointing from parent to child
CalVector direction = delta/length;
CalCoreBone* child_bone = skeleton->getCoreSkeleton()->getCoreBone( *it );
CalVector rest_delta = bone->getCoreBone()->getTranslationAbsolute() - child_bone->getTranslationAbsolute();
float desired_length = rest_delta.length();
float delta_length = desired_length - length;
// balance according to weight_centre
float weight_centre = getWeightCentre(next_id);
// move
b_c += weight_centre * delta_length * direction;
b_p -= (1.0f-weight_centre) * delta_length * direction;
//printf("%s %f (%f %f %f), ", child_bone->getName().c_str(), delta_length, b_p.x, b_p.y, b_p.z );
}
//printf("\n");
// now average
CalVector average;
for ( int i=0; i<results.size(); i++ )
{
average += results[i];
}
average /= results.size();
// store
world_positions[next_id] = average;
// add parent
queue.push_back( parent_id );
}
}
else
{
// children.size() is exactly 1
assert( children.size()==1 );
int child_id = children.front();
// child_pos
CalVector& b_c = world_positions[child_id];
// current pos
CalVector& b_p = world_positions[next_id];
// now, the bone is the wrong length. correct its length to fulfil size constraint.
float desired_length = getBoneLength(next_id);
CalVector delta = b_c - b_p;
float length = delta.length();
length = max( 0.00001f, length );
length = min( desired_length*1.5f, length );
// pointing from parent to child
CalVector direction = delta/length;
float delta_length = desired_length - length;
// balance according to weight_centre
float weight_centre = getWeightCentre(next_id);
// move
b_c += weight_centre * delta_length * direction;
b_p -= (1.0f-weight_centre) * delta_length * direction;
// add parent
int parent_id = bone->getCoreBone()->getParentId();
if ( parent_id != -1 )
queue.push_back( parent_id );
}
}
iterations--;
}
}
void CalSpringSystem::calculateVertices(CalSubmesh *pSubmesh, float deltaTime)
{
// get the vertex vector of the submesh
std::vector<CalVector>& vectorVertex = pSubmesh->getVectorVertex();
// get the physical property vector of the submesh
std::vector<CalSubmesh::PhysicalProperty>& vectorPhysicalProperty = pSubmesh->getVectorPhysicalProperty();
// get the physical property vector of the core submesh
std::vector<CalCoreSubmesh::PhysicalProperty>& vectorCorePhysicalProperty = pSubmesh->getCoreSubmesh()->getVectorPhysicalProperty();
// loop through all the vertices
int vertexId;
for(vertexId = 0; vertexId < (int)vectorVertex.size(); ++vertexId)
{
// get the vertex
CalVector& vertex = vectorVertex[vertexId];
// get the physical property of the vertex
CalSubmesh::PhysicalProperty& physicalProperty = vectorPhysicalProperty[vertexId];
// get the physical property of the core vertex
CalCoreSubmesh::PhysicalProperty& corePhysicalProperty = vectorCorePhysicalProperty[vertexId];
// store current position for later use
CalVector position;
position = physicalProperty.position;
// only take vertices with a weight > 0 into account
if(corePhysicalProperty.weight > 0.0f)
{
// do the Verlet step
physicalProperty.position += (position - physicalProperty.positionOld) * 0.99f + physicalProperty.force / corePhysicalProperty.weight * deltaTime * deltaTime;
}
else
{
physicalProperty.position = vectorVertex[vertexId];
}
// make the current position the old one
physicalProperty.positionOld = position;
// set the new position of the vertex
vertex = physicalProperty.position;
// clear the accumulated force on the vertex
physicalProperty.force.clear();
}
// get the spring vector of the core submesh
std::vector<CalCoreSubmesh::Spring>& vectorSpring = pSubmesh->getCoreSubmesh()->getVectorSpring();
// iterate a few times to relax the constraints
int iterationCount;
#define ITERATION_COUNT 2
for(iterationCount = 0; iterationCount < ITERATION_COUNT; ++iterationCount)
{
// loop through all the springs
std::vector<CalCoreSubmesh::Spring>::iterator iteratorSpring;
for(iteratorSpring = vectorSpring.begin(); iteratorSpring != vectorSpring.end(); ++iteratorSpring)
{
// get the spring
CalCoreSubmesh::Spring& spring = *iteratorSpring;
// compute the difference between the two spring vertices
CalVector distance;
distance = vectorVertex[spring.vertexId[1]] - vectorVertex[spring.vertexId[0]];
// get the current length of the spring
float length;
length = distance.length();
if(length > 0.0f)
{
float factor[2];
factor[0] = (length - spring.idleLength) / length;
factor[1] = factor[0];
if(vectorCorePhysicalProperty[spring.vertexId[0]].weight > 0.0f)
{
factor[0] /= 2.0f;
factor[1] /= 2.0f;
}
else
{
factor[0] = 0.0f;
}
if(vectorCorePhysicalProperty[spring.vertexId[1]].weight <= 0.0f)
{
factor[0] *= 2.0f;
factor[1] = 0.0f;
}
vectorVertex[spring.vertexId[0]] += distance * factor[0];
vectorPhysicalProperty[spring.vertexId[0]].position = vectorVertex[spring.vertexId[0]];
vectorVertex[spring.vertexId[1]] -= distance * factor[1];
vectorPhysicalProperty[spring.vertexId[1]].position = vectorVertex[spring.vertexId[1]];
}
}
}
/* DEBUG-CODE ********************
CalVector spherePosition(Sphere.x, Sphere.y, Sphere.z);
float sphereRadius = Sphere.radius;
// loop through all the vertices
for(vertexId = 0; vertexId < (int)vectorVertex.size(); ++vertexId)
{
// get the vertex
CalVector& vertex = vectorVertex[vertexId];
// get the physical property of the vertex
CalSubmesh::PhysicalProperty& physicalProperty = vectorPhysicalProperty[vertexId];
// get the physical property of the core vertex
CalCoreSubmesh::PhysicalProperty& corePhysicalProperty = vectorCorePhysicalProperty[vertexId];
// only take vertices with a weight > 0 into account
if(corePhysicalProperty.weight > 0.0f)
{
CalVector position;
position = physicalProperty.position;
position -= spherePosition;
float length;
length = position.normalize();
if(length < sphereRadius)
{
position *= sphereRadius;
position += spherePosition;
physicalProperty.position = position;
physicalProperty.positionOld = position;
vertex = physicalProperty.position;
}
}
}
*********************************/
}
void IKCharacter::setup( CalSkeleton* cal_skel, bool _auto_root_follow )
{
skeleton = cal_skel;
auto_root_follow = _auto_root_follow;
// find leaf bones
deque<int> current;
vector<int> roots = cal_skel->getCoreSkeleton()->getVectorRootCoreBoneId();
current.insert( current.begin(), roots.begin(), roots.end() );
while( !current.empty() )
{
CalCoreBone* check = skeleton->getCoreSkeleton()->getCoreBone( current.front() );
list<int> children = check->getListChildId();
if ( children.size()==0 )
{
// this is a leaf
leaf_bones.push_back( current.front() );
printf("found leaf: %s\n", skeleton->getCoreSkeleton()->getCoreBone( leaf_bones.back() )->getName().c_str() );
}
else
{
// not a leaf
current.insert( current.end(), children.begin(), children.end() );
}
// store length
// default non-zero
bone_lengths[current.front()] = 0.0001f;
if ( check->getParentId()!= -1 )
{
CalCoreBone* parent = skeleton->getCoreSkeleton()->getCoreBone( check->getParentId() );
CalVector delta = check->getTranslationAbsolute() - parent->getTranslationAbsolute();
bone_lengths[check->getParentId()] = delta.length();
}
// cached rotations
debug_cached_rotations[current.front()] = CalQuaternion();
current.pop_front();
}
// set all weights to default
vector<CalCoreBone*> all_bones = skeleton->getCoreSkeleton()->getVectorCoreBone();
for( int i=0; i<all_bones.size() ;i++ )
{
weight_centres[all_bones[i]->getId()] = 0.5f;
}
// release all leaves
for ( int i=0; i<leaf_bones.size() ;i++ )
{
weight_centres[leaf_bones[i]] = 0;
}
// pin all roots
for ( int i=0; i<roots.size(); i++ )
{
weight_centres[roots[i]] = 1;
}
// fetch world positions from Cal3D model
pullWorldPositions();
// set targets
/*
for ( int i=0; i<leaf_bones.size(); i++ )
{
CalVector p = world_positions[leaf_bones[i]];
setTarget( leaf_bones[i], ofxVec3f(p.x,p.y,p.z) );
}*/
//
setupMagicIgnoringRotationOffsets();
}