csQuaternion csQuaternion::SLerp (const csQuaternion& q2, float t) const
{
float omega, cosom, invsinom, scale0, scale1;
csQuaternion quato(q2);
// decide if one of the quaternions is backwards
float a = (*this-q2).SquaredNorm ();
float b = (*this+q2).SquaredNorm ();
if (a > b)
{
quato = -q2;
}
// Calculate dot between quats
cosom = Dot (quato);
// Make sure the two quaternions are not exactly opposite? (within a little
// slop).
if (cosom > -0.9998f)
{
// Are they more than a little bit different? Avoid a divided by zero
// and lerp if not.
if (cosom < 0.9998f)
{
// Yes, do a slerp
omega = acosf (cosom);
invsinom = 1.0f / sinf (omega);
scale0 = sinf ((1.0f - t) * omega) * invsinom;
scale1 = sinf (t * omega) * invsinom;
}
else
{
// Not a very big difference, do a lerp
scale0 = 1.0f - t;
scale1 = t;
}
return csQuaternion (
scale0 * v.x + scale1 * quato.v.x,
scale0 * v.y + scale1 * quato.v.y,
scale0 * v.z + scale1 * quato.v.z,
scale0 * w + scale1 * quato.w);
}
// The quaternions are nearly opposite so to avoid a divided by zero error
// Calculate a perpendicular quaternion and slerp that direction
scale0 = sin ((1.0f - t) * PI);
scale1 = sin (t * PI);
return csQuaternion (
scale0 * v.x + scale1 * -quato.v.y,
scale0 * v.y + scale1 * quato.v.x,
scale0 * v.z + scale1 * -quato.w,
scale0 * w + scale1 * quato.v.z);
}
csQuaternion csQuaternion::Log () const
{
// q = w + v, w is real, v is complex vector
// let u be v / |v|
// log(q) = 1/2*log(|q|) + u*atan(|v|/ w)
float vNorm = v.Norm ();
float qSqNorm = SquaredNorm ();
float vCoeff;
if (vNorm > 0.0f)
vCoeff = atan2f (vNorm, w) / vNorm;
else
vCoeff = 0;
return csQuaternion (v * vCoeff, 0.5f * logf (qSqNorm));
}
csQuaternion csQuaternion::Exp () const
{
// q = w + v, w is real, v is complex vector
// let u be v / |v|
// exp(q) = exp(w) * (cos(|v|), u*sin(|v|))
float vNorm = v.Norm ();
float expW = expf (w);
float vCoeff;
if (vNorm > 0.0f)
vCoeff = expW * sinf (vNorm) / vNorm;
else
vCoeff = 0;
return csQuaternion (v * vCoeff, expW * cosf (vNorm));
}
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;
}
}
