float *findRotChannel(skcHeader_t *h, const char *name, int frameNum, int parentIndex) {
#if 0
char channelName[32];
strcpy(channelName,name);
strcat(channelName," rot");
return getChannelValue(h,channelName,frameNum);
#else
static int i = 0;
static quat_t qs[1024];
float *q;
char channelName[32];
float *f;
float len;
i++;
i %= 1024;
q = qs[i];
strcpy(channelName,name);
strcat(channelName," rot");
f = getChannelValue(h,channelName,frameNum);
if(f == 0) {
//return quat_identity;
QuatSet(q,0,0,0,-1);
} else {
QuatCopy(f,q);
}
//QuatInverse(q);
////if(parentIndex == -1)
// QuatInverse(q);
len = QuatNormalize(q);
if(abs(len-1.f) > 0.1) {
T_Error("Non-normalized quat in skc file (%f)\n",len);
}
#if 1
FixQuatForMD5_P(q);
#endif
return q;
#endif
}
//----------------------------------------------------------
// クォータニオンの球面線形補間
// Q1をQ2方向に補間する
//----------------------------------------------------------
Quat &QuatSlerp( Quat &Qo, const Quat &Q1, const Quat &Q2, float t )
{
Quat q1( Q1 );
Quat q2( Q2 );
float theta;
theta = QuatDot( Q1, Q2 );
if( theta < 0 )
{
theta = -theta;
q2 *= -1;
}
if( theta > 1.0f )
{
theta = 1.0f;
}
if( theta < SLERP_MAX )
{
theta = acosf( theta );
q1 *= sinf( theta * (1.0f - t) );
q2 *= sinf( theta * t );
q1 += q2;
Qo = q1 / sinf( theta );
}
else
{
q1 += q2 * t;
QuatNormalize( Qo, q1 );
}
return Qo;
}
/*
==============
RE_BuildSkeleton
==============
*/
int RE_BuildSkeleton(refSkeleton_t * skel, qhandle_t hAnim, int startFrame, int endFrame, float frac, qboolean clearOrigin)
{
skelAnimation_t *skelAnim;
skelAnim = R_GetAnimationByHandle(hAnim);
if(skelAnim->type == AT_MD5 && skelAnim->md5)
{
int i;
md5Animation_t *anim;
md5Channel_t *channel;
md5Frame_t *newFrame, *oldFrame;
vec3_t newOrigin, oldOrigin, lerpedOrigin;
quat_t newQuat, oldQuat, lerpedQuat;
int componentsApplied;
anim = skelAnim->md5;
// Validate the frames so there is no chance of a crash.
// This will write directly into the entity structure, so
// when the surfaces are rendered, they don't need to be
// range checked again.
/*
if((startFrame >= anim->numFrames) || (startFrame < 0) || (endFrame >= anim->numFrames) || (endFrame < 0))
{
ri.Printf(PRINT_DEVELOPER, "RE_BuildSkeleton: no such frame %d to %d for '%s'\n", startFrame, endFrame, anim->name);
//startFrame = 0;
//endFrame = 0;
}
*/
Q_clamp(startFrame, 0, anim->numFrames - 1);
Q_clamp(endFrame, 0, anim->numFrames - 1);
// compute frame pointers
oldFrame = &anim->frames[startFrame];
newFrame = &anim->frames[endFrame];
// calculate a bounding box in the current coordinate system
for(i = 0; i < 3; i++)
{
skel->bounds[0][i] =
oldFrame->bounds[0][i] < newFrame->bounds[0][i] ? oldFrame->bounds[0][i] : newFrame->bounds[0][i];
skel->bounds[1][i] =
oldFrame->bounds[1][i] > newFrame->bounds[1][i] ? oldFrame->bounds[1][i] : newFrame->bounds[1][i];
}
for(i = 0, channel = anim->channels; i < anim->numChannels; i++, channel++)
{
// set baseframe values
VectorCopy(channel->baseOrigin, newOrigin);
VectorCopy(channel->baseOrigin, oldOrigin);
QuatCopy(channel->baseQuat, newQuat);
QuatCopy(channel->baseQuat, oldQuat);
componentsApplied = 0;
// update tranlation bits
if(channel->componentsBits & COMPONENT_BIT_TX)
{
oldOrigin[0] = oldFrame->components[channel->componentsOffset + componentsApplied];
newOrigin[0] = newFrame->components[channel->componentsOffset + componentsApplied];
componentsApplied++;
}
if(channel->componentsBits & COMPONENT_BIT_TY)
{
oldOrigin[1] = oldFrame->components[channel->componentsOffset + componentsApplied];
newOrigin[1] = newFrame->components[channel->componentsOffset + componentsApplied];
componentsApplied++;
}
if(channel->componentsBits & COMPONENT_BIT_TZ)
{
oldOrigin[2] = oldFrame->components[channel->componentsOffset + componentsApplied];
newOrigin[2] = newFrame->components[channel->componentsOffset + componentsApplied];
componentsApplied++;
}
// update quaternion rotation bits
if(channel->componentsBits & COMPONENT_BIT_QX)
{
((vec_t *) oldQuat)[0] = oldFrame->components[channel->componentsOffset + componentsApplied];
((vec_t *) newQuat)[0] = newFrame->components[channel->componentsOffset + componentsApplied];
componentsApplied++;
}
if(channel->componentsBits & COMPONENT_BIT_QY)
{
((vec_t *) oldQuat)[1] = oldFrame->components[channel->componentsOffset + componentsApplied];
((vec_t *) newQuat)[1] = newFrame->components[channel->componentsOffset + componentsApplied];
componentsApplied++;
}
if(channel->componentsBits & COMPONENT_BIT_QZ)
{
((vec_t *) oldQuat)[2] = oldFrame->components[channel->componentsOffset + componentsApplied];
((vec_t *) newQuat)[2] = newFrame->components[channel->componentsOffset + componentsApplied];
}
QuatCalcW(oldQuat);
QuatNormalize(oldQuat);
QuatCalcW(newQuat);
QuatNormalize(newQuat);
#if 1
VectorLerp(oldOrigin, newOrigin, frac, lerpedOrigin);
QuatSlerp(oldQuat, newQuat, frac, lerpedQuat);
#else
VectorCopy(newOrigin, lerpedOrigin);
QuatCopy(newQuat, lerpedQuat);
#endif
// copy lerped information to the bone + extra data
skel->bones[i].parentIndex = channel->parentIndex;
if(channel->parentIndex < 0 && clearOrigin)
{
VectorClear(skel->bones[i].origin);
QuatClear(skel->bones[i].rotation);
// move bounding box back
VectorSubtract(skel->bounds[0], lerpedOrigin, skel->bounds[0]);
VectorSubtract(skel->bounds[1], lerpedOrigin, skel->bounds[1]);
}
else
{
VectorCopy(lerpedOrigin, skel->bones[i].origin);
}
QuatCopy(lerpedQuat, skel->bones[i].rotation);
#if defined(REFBONE_NAMES)
Q_strncpyz(skel->bones[i].name, channel->name, sizeof(skel->bones[i].name));
#endif
}
skel->numBones = anim->numChannels;
skel->type = SK_RELATIVE;
return qtrue;
}
else if(skelAnim->type == AT_PSA && skelAnim->psa)
{
int i;
psaAnimation_t *anim;
axAnimationKey_t *newKey, *oldKey;
axReferenceBone_t *refBone;
vec3_t newOrigin, oldOrigin, lerpedOrigin;
quat_t newQuat, oldQuat, lerpedQuat;
refSkeleton_t skeleton;
anim = skelAnim->psa;
Q_clamp(startFrame, 0, anim->info.numRawFrames - 1);
Q_clamp(endFrame, 0, anim->info.numRawFrames - 1);
ClearBounds(skel->bounds[0], skel->bounds[1]);
skel->numBones = anim->info.numBones;
for(i = 0, refBone = anim->bones; i < anim->info.numBones; i++, refBone++)
{
oldKey = &anim->keys[startFrame * anim->info.numBones + i];
newKey = &anim->keys[endFrame * anim->info.numBones + i];
VectorCopy(newKey->position, newOrigin);
VectorCopy(oldKey->position, oldOrigin);
QuatCopy(newKey->quat, newQuat);
QuatCopy(oldKey->quat, oldQuat);
//QuatCalcW(oldQuat);
//QuatNormalize(oldQuat);
//QuatCalcW(newQuat);
//QuatNormalize(newQuat);
VectorLerp(oldOrigin, newOrigin, frac, lerpedOrigin);
QuatSlerp(oldQuat, newQuat, frac, lerpedQuat);
// copy lerped information to the bone + extra data
skel->bones[i].parentIndex = refBone->parentIndex;
if(refBone->parentIndex < 0 && clearOrigin)
{
VectorClear(skel->bones[i].origin);
QuatClear(skel->bones[i].rotation);
// move bounding box back
VectorSubtract(skel->bounds[0], lerpedOrigin, skel->bounds[0]);
VectorSubtract(skel->bounds[1], lerpedOrigin, skel->bounds[1]);
}
else
{
VectorCopy(lerpedOrigin, skel->bones[i].origin);
}
QuatCopy(lerpedQuat, skel->bones[i].rotation);
#if defined(REFBONE_NAMES)
Q_strncpyz(skel->bones[i].name, refBone->name, sizeof(skel->bones[i].name));
#endif
// calculate absolute values for the bounding box approximation
VectorCopy(skel->bones[i].origin, skeleton.bones[i].origin);
QuatCopy(skel->bones[i].rotation, skeleton.bones[i].rotation);
if(refBone->parentIndex >= 0)
{
vec3_t rotated;
quat_t quat;
refBone_t *parent;
refBone_t *bone;
bone = &skeleton.bones[i];
parent = &skeleton.bones[refBone->parentIndex];
QuatTransformVector(parent->rotation, bone->origin, rotated);
VectorAdd(parent->origin, rotated, bone->origin);
QuatMultiply1(parent->rotation, bone->rotation, quat);
QuatCopy(quat, bone->rotation);
AddPointToBounds(bone->origin, skel->bounds[0], skel->bounds[1]);
}
}
skel->numBones = anim->info.numBones;
skel->type = SK_RELATIVE;
return qtrue;
}
//ri.Printf(PRINT_WARNING, "RE_BuildSkeleton: bad animation '%s' with handle %i\n", anim->name, hAnim);
// FIXME: clear existing bones and bounds?
return qfalse;
}
/*
==============
RE_BuildSkeleton
==============
*/
int RE_BuildSkeleton( refSkeleton_t *skel, qhandle_t hAnim, int startFrame, int endFrame, float frac, bool clearOrigin )
{
skelAnimation_t *skelAnim;
skelAnim = R_GetAnimationByHandle( hAnim );
if ( skelAnim->type == animType_t::AT_IQM && skelAnim->iqm ) {
return IQMBuildSkeleton( skel, skelAnim, startFrame, endFrame, frac );
}
else if ( skelAnim->type == animType_t::AT_MD5 && skelAnim->md5 )
{
int i;
md5Animation_t *anim;
md5Channel_t *channel;
md5Frame_t *newFrame, *oldFrame;
vec3_t newOrigin, oldOrigin, lerpedOrigin;
quat_t newQuat, oldQuat, lerpedQuat;
int componentsApplied;
anim = skelAnim->md5;
// Validate the frames so there is no chance of a crash.
// This will write directly into the entity structure, so
// when the surfaces are rendered, they don't need to be
// range checked again.
/*
if((startFrame >= anim->numFrames) || (startFrame < 0) || (endFrame >= anim->numFrames) || (endFrame < 0))
{
Log::Debug("RE_BuildSkeleton: no such frame %d to %d for '%s'\n", startFrame, endFrame, anim->name);
//startFrame = 0;
//endFrame = 0;
}
*/
startFrame = Math::Clamp( startFrame, 0, anim->numFrames - 1 );
endFrame = Math::Clamp( endFrame, 0, anim->numFrames - 1 );
// compute frame pointers
oldFrame = &anim->frames[ startFrame ];
newFrame = &anim->frames[ endFrame ];
// calculate a bounding box in the current coordinate system
for ( i = 0; i < 3; i++ )
{
skel->bounds[ 0 ][ i ] =
oldFrame->bounds[ 0 ][ i ] < newFrame->bounds[ 0 ][ i ] ? oldFrame->bounds[ 0 ][ i ] : newFrame->bounds[ 0 ][ i ];
skel->bounds[ 1 ][ i ] =
oldFrame->bounds[ 1 ][ i ] > newFrame->bounds[ 1 ][ i ] ? oldFrame->bounds[ 1 ][ i ] : newFrame->bounds[ 1 ][ i ];
}
for ( i = 0, channel = anim->channels; i < anim->numChannels; i++, channel++ )
{
// set baseframe values
VectorCopy( channel->baseOrigin, newOrigin );
VectorCopy( channel->baseOrigin, oldOrigin );
QuatCopy( channel->baseQuat, newQuat );
QuatCopy( channel->baseQuat, oldQuat );
componentsApplied = 0;
// update tranlation bits
if ( channel->componentsBits & COMPONENT_BIT_TX )
{
oldOrigin[ 0 ] = oldFrame->components[ channel->componentsOffset + componentsApplied ];
newOrigin[ 0 ] = newFrame->components[ channel->componentsOffset + componentsApplied ];
componentsApplied++;
}
if ( channel->componentsBits & COMPONENT_BIT_TY )
{
oldOrigin[ 1 ] = oldFrame->components[ channel->componentsOffset + componentsApplied ];
newOrigin[ 1 ] = newFrame->components[ channel->componentsOffset + componentsApplied ];
componentsApplied++;
}
if ( channel->componentsBits & COMPONENT_BIT_TZ )
{
oldOrigin[ 2 ] = oldFrame->components[ channel->componentsOffset + componentsApplied ];
newOrigin[ 2 ] = newFrame->components[ channel->componentsOffset + componentsApplied ];
componentsApplied++;
}
// update quaternion rotation bits
if ( channel->componentsBits & COMPONENT_BIT_QX )
{
( ( vec_t * ) oldQuat ) [ 0 ] = oldFrame->components[ channel->componentsOffset + componentsApplied ];
( ( vec_t * ) newQuat ) [ 0 ] = newFrame->components[ channel->componentsOffset + componentsApplied ];
componentsApplied++;
}
if ( channel->componentsBits & COMPONENT_BIT_QY )
{
( ( vec_t * ) oldQuat ) [ 1 ] = oldFrame->components[ channel->componentsOffset + componentsApplied ];
( ( vec_t * ) newQuat ) [ 1 ] = newFrame->components[ channel->componentsOffset + componentsApplied ];
componentsApplied++;
}
if ( channel->componentsBits & COMPONENT_BIT_QZ )
{
( ( vec_t * ) oldQuat ) [ 2 ] = oldFrame->components[ channel->componentsOffset + componentsApplied ];
( ( vec_t * ) newQuat ) [ 2 ] = newFrame->components[ channel->componentsOffset + componentsApplied ];
}
QuatCalcW( oldQuat );
QuatNormalize( oldQuat );
QuatCalcW( newQuat );
QuatNormalize( newQuat );
#if 1
VectorLerp( oldOrigin, newOrigin, frac, lerpedOrigin );
QuatSlerp( oldQuat, newQuat, frac, lerpedQuat );
#else
VectorCopy( newOrigin, lerpedOrigin );
QuatCopy( newQuat, lerpedQuat );
#endif
// copy lerped information to the bone + extra data
skel->bones[ i ].parentIndex = channel->parentIndex;
if ( channel->parentIndex < 0 && clearOrigin )
{
VectorClear( skel->bones[ i ].t.trans );
QuatClear( skel->bones[ i ].t.rot );
// move bounding box back
VectorSubtract( skel->bounds[ 0 ], lerpedOrigin, skel->bounds[ 0 ] );
VectorSubtract( skel->bounds[ 1 ], lerpedOrigin, skel->bounds[ 1 ] );
}
else
{
VectorCopy( lerpedOrigin, skel->bones[ i ].t.trans );
}
QuatCopy( lerpedQuat, skel->bones[ i ].t.rot );
skel->bones[ i ].t.scale = 1.0f;
#if defined( REFBONE_NAMES )
Q_strncpyz( skel->bones[ i ].name, channel->name, sizeof( skel->bones[ i ].name ) );
#endif
}
skel->numBones = anim->numChannels;
skel->type = refSkeletonType_t::SK_RELATIVE;
return true;
}
// FIXME: clear existing bones and bounds?
return false;
}
float *AttitudeUpdate(float dt,float _beta, float _zeta, float a_x,float a_y, float a_z,float w_x, float w_y, float w_z,float m_x, float m_y, float m_z)
{
float qConj[4];
float unbiasedGyro[4];
float qDot[4];
float beta,zeta;
float F[6];
float J[6*4];
float step[4];
float North,East,Down;
float b2,b4;
float q1 = q[0]; float q2 = q[1]; float q3 = q[2]; float q4 = q[3];
float _error[4];
float accel[] = {0,-a_x,-a_y,-a_z};
float mag[] = {0,m_x,m_y,m_z};
// Direction cosine matrix from quaternion
float qdcm13 = 2.0f*(q2*q4 - q1*q3);
float qdcm23 = 2.0f*(q3*q4 + q1*q2);
float qdcm33 = 2.0f*(0.5f - q2*q2 - q3*q3);
float qdcm12 = 2.0f*(q2*q3 + q1*q4);
float qdcm22 = 2.0f*(0.5f - q2*q2 - q4*q4);
float qdcm32 = 2.0f*(q3*q4 - q1*q2);
float qdcm11 = 2.0f*(0.5f - q3*q3 - q4*q4);
float qdcm21 = 2.0f*(q2*q3 - q1*q4);
float qdcm31 = 2.0f*(q2*q4 + q1*q3);
counter += dt;
QuatNormalize(accel);
QuatNormalize(mag);
// Earth's field
North = qdcm11*mag[1] + qdcm21*mag[2] + qdcm31*mag[3];
East = qdcm12*mag[1] + qdcm22*mag[2] + qdcm32*mag[3];
Down = qdcm13*mag[1] + qdcm23*mag[2] + qdcm33*mag[3];
b2 = sqrtf(North*North + East*East);
b4 = Down;
// 6 x 1
F[0] = qdcm13 - accel[1];
F[1] = qdcm23 - accel[2];
F[2] = qdcm33 - accel[3];
F[3] = b2*qdcm11 + b4*qdcm13 - mag[1];
F[4] = b2*qdcm21 + b4*qdcm23 - mag[2];
F[5] = b2*qdcm31 + b4*qdcm33 - mag[3];
// 6 x 4
J[0] = -2*q3; J[1] = 2*q4; J[2] = -2*q1; J[3] = 2*q2;
J[4] = 2*q2; J[5] = 2*q1; J[6] = 2*q4; J[7] = 2*q3;
J[8] = 0; J[9] = -4*q2; J[10]= -4*q3; J[11] = 0;
J[12]=-2*b4*q3; J[13]= 2*b4*q4; J[14]=-4*b2*q3-2*b4*q1; J[15]= -4*b2*q4+2*b4*q2;
J[16]=-2*b2*q4+2*b4*q2; J[17]= 2*b2*q3+2*b4*q1;J[18]= 2*b2*q2+2*b4*q4; J[19]=-2*b2*q1+2*b4*q3;
J[20]=2*b2*q3; J[21]= 2*b2*q4-4*b4*q2;J[22]= 2*b2*q1-4*b4*q3; J[23] =2*b2*q2;
//float Jt[4*6];
//vDSP_mtrans(J,1,Jt,1,4,6);
// step = J'*F
// 4 x 6 * 6 x 1
//vDSP_mmul( Jt,1,F,1,step,1,4,1,6);
step[0] = J[0]*F[0] + J[4]*F[1] + J[8]*F[2] + J[12]*F[3] + J[16]*F[4] + J[20]*F[5];
step[1] = J[1]*F[0] + J[5]*F[1] + J[9]*F[2] + J[13]*F[3] + J[17]*F[4] + J[21]*F[5];
step[2] = J[2]*F[0] + J[6]*F[1] + J[10]*F[2] + J[14]*F[3] + J[18]*F[4] + J[22]*F[5];
step[3] = J[3]*F[0] + J[7]*F[1] + J[11]*F[2] + J[15]*F[3] + J[19]*F[4] + J[23]*F[5];
// normalize step
QuatNormalize(step);
qConj[0] = q[0]; qConj[1] = -q[1]; qConj[2]=-q[2]; qConj[3]=-q[3];
// Error
QuatMultiply(qConj,step,_error);
QuatScale(_error,(2.0f*dt));
// update gyro biases
w_bx += zeta*_error[1]; w_by += zeta*_error[2]; w_bz += zeta*_error[3];
// quaternion dynamics
unbiasedGyro[0] = 0; unbiasedGyro[1] = (w_x-w_bx); unbiasedGyro[2]= (w_y-w_by); unbiasedGyro[3] = (w_z-w_bz);
QuatMultiply(q,unbiasedGyro,qDot);
// higher gain during startup
if(counter < 5.0f)
{
beta = 1.0f;
zeta = 1.0f;
}
else
{
beta = _beta;
zeta = _zeta;
}
qDot[0] = 0.5f*qDot[0] - beta*step[0];
qDot[1] = 0.5f*qDot[1] - beta*step[1];
qDot[2] = 0.5f*qDot[2] - beta*step[2];
qDot[3] = 0.5f*qDot[3] - beta*step[3];
q[0] += qDot[0]*dt;
q[1] += qDot[1]*dt;
q[2] += qDot[2]*dt;
q[3] += qDot[3]*dt;
QuatNormalize(q);
return q;
}
/*
================
CG_AddFragment
================
*/
void CG_AddFragment(localEntity_t * le)
{
vec3_t newOrigin;
trace_t trace;
if(le->pos.trType == TR_STATIONARY)
{
// sink into the ground if near the removal time
int t;
float oldZ;
t = le->endTime - cg.time;
if(t < SINK_TIME)
{
// we must use an explicit lighting origin, otherwise the
// lighting would be lost as soon as the origin went
// into the ground
VectorCopy(le->refEntity.origin, le->refEntity.lightingOrigin);
le->refEntity.renderfx |= RF_LIGHTING_ORIGIN;
oldZ = le->refEntity.origin[2];
le->refEntity.origin[2] -= 16 * (1.0 - (float)t / SINK_TIME);
trap_R_AddRefEntityToScene(&le->refEntity);
le->refEntity.origin[2] = oldZ;
}
else
{
trap_R_AddRefEntityToScene(&le->refEntity);
}
return;
}
// calculate new position
BG_EvaluateTrajectory(&le->pos, cg.time, newOrigin);
// trace a line from previous position to new position
CG_Trace(&trace, le->refEntity.origin, NULL, NULL, newOrigin, -1, CONTENTS_SOLID);
if(trace.fraction == 1.0)
{
// still in free fall
VectorCopy(newOrigin, le->refEntity.origin);
if(le->leFlags & LEF_TUMBLE)
{
#if 0
vec3_t angles;
BG_EvaluateTrajectory(&le->angles, cg.time, angles);
AnglesToAxis(angles, le->refEntity.axis);
#else
// Tr3B - new quaternion code
quat_t qrot;
// angular rotation for this frame
float angle = le->angVel * (cg.time - le->angles.trTime) * 0.001 / 2;
// create the rotation quaternion
qrot[3] = cos(angle); // real part
VectorScale(le->rotAxis, sin(angle), qrot); // imaginary part
QuatNormalize(qrot);
// create the new orientation
QuatMultiply0(le->quatOrient, qrot);
// apply the combined previous rotations around other axes
QuatMultiply1(le->quatOrient, le->quatRot, qrot);
// convert the orientation into the form the renderer wants
QuatToAxis(qrot, le->refEntity.axis);
le->angles.trTime = cg.time;
#endif
}
trap_R_AddRefEntityToScene(&le->refEntity);
// add a blood trail
if(le->leBounceSoundType == LEBS_BLOOD)
{
CG_BloodTrail(le);
}
return;
}
// if it is in a nodrop zone, remove it
// this keeps gibs from waiting at the bottom of pits of death
// and floating levels
if(trap_CM_PointContents(trace.endpos, 0) & CONTENTS_NODROP)
{
CG_FreeLocalEntity(le);
return;
}
// leave a mark
CG_FragmentBounceMark(le, &trace);
// do a bouncy sound
CG_FragmentBounceSound(le, &trace);
// reflect the velocity on the trace plane
CG_ReflectVelocity(le, &trace);
trap_R_AddRefEntityToScene(&le->refEntity);
}
