union quat *quat_lerp(union quat *qo, const union quat *qfrom, const union quat *qto, float t)
{
double cosom = quat_dot(qfrom, qto);
/* qto = qfrom or qto = -qfrom so no rotation to slerp */
if (cosom >= 1.0) {
quat_copy(qo, qfrom);
return qo;
}
/* adjust for shortest path */
union quat to1;
if (cosom < 0.0) {
to1.v.x = -qto->v.x;
to1.v.y = -qto->v.y;
to1.v.z = -qto->v.z;
to1.v.w = -qto->v.w;
} else {
quat_copy(&to1, qto);
}
double scale0 = 1.0 - t;
double scale1 = t;
/* calculate final values */
qo->v.x = scale0 * qfrom->v.x + scale1 * to1.v.x;
qo->v.y = scale0 * qfrom->v.y + scale1 * to1.v.y;
qo->v.z = scale0 * qfrom->v.z + scale1 * to1.v.z;
qo->v.w = scale0 * qfrom->v.w + scale1 * to1.v.w;
return qo;
}
static int Pow(lua_State *L)
{
quat_t q, dst;
lua_Integer n, i;
checkquat(L, 1, q);
n = luaL_checkinteger(L, 2);
if(n == 0)
{
quat_clear(dst); dst[0]=1;
}
else
{
if(n > 0)
quat_copy(dst, q);
else
{
quat_inv(dst, q);
quat_copy(q, dst);
n = -n;
}
for(i = 0; i < (n-1); i++)
quat_mulby(dst, q);
}
return pushquat(L, dst);
}
/*
==============
RE_BlendSkeleton
==============
*/
int RE_BlendSkeleton(refSkeleton_t *skel, const refSkeleton_t *blend, float frac)
{
int i;
vec3_t lerpedOrigin;
quat_t lerpedQuat;
vec3_t bounds[2];
if (skel->numBones != blend->numBones)
{
Ren_Warning("RE_BlendSkeleton: different number of bones %d != %d\n", skel->numBones, blend->numBones);
return qfalse;
}
// lerp between the 2 bone poses
for (i = 0; i < skel->numBones; i++)
{
VectorLerp(skel->bones[i].origin, blend->bones[i].origin, frac, lerpedOrigin);
quat_slerp(skel->bones[i].rotation, blend->bones[i].rotation, frac, lerpedQuat);
VectorCopy(lerpedOrigin, skel->bones[i].origin);
quat_copy(lerpedQuat, skel->bones[i].rotation);
}
// calculate a bounding box in the current coordinate system
for (i = 0; i < 3; i++)
{
bounds[0][i] = skel->bounds[0][i] < blend->bounds[0][i] ? skel->bounds[0][i] : blend->bounds[0][i];
bounds[1][i] = skel->bounds[1][i] > blend->bounds[1][i] ? skel->bounds[1][i] : blend->bounds[1][i];
}
VectorCopy(bounds[0], skel->bounds[0]);
VectorCopy(bounds[1], skel->bounds[1]);
return qtrue;
}
void QuatMultiply0(quat_t qa, const quat_t qb)
{
quat_t tmp;
quat_copy(qa, tmp);
QuatMultiply1(tmp, qb, qa);
}
void
quat_q_from_R (quaternion_t q, const double *R, const int ldim)
{
double c2;
c2 = ((R[0] + R[ldim + 1] + R[2*ldim + 2]) + 1.0) / 4.0;
if (c2 < 0.0 || 1.0 < c2)
{
quat_copy (q, quat_one);
return;
}
quat_re (q) = sqrt (c2);
if (quat_re (q) > QUAT_EPS)
{
quat_im (q, 0) = (R[ldim + 2] - R[2*ldim + 1]) / 4.0 / quat_re (q);
quat_im (q, 1) = (R[2*ldim + 0] - R[2]) / 4.0 / quat_re (q);
quat_im (q, 2) = (R[1] - R[ldim]) / 4.0 / quat_re (q);
}
else
{
quat_im (q, 0) = sqrt((R[0] + 1.0)/2.0 - c2);
quat_im (q, 1) = sqrt((R[ldim + 1] + 1.0)/2.0 - c2);
quat_im (q, 2) = sqrt((R[2*ldim + 2] + 1.0)/2.0 - c2);
}
quat_normalize (q);
}
void quat_add_to(union quat *o, const union quat *q)
{
union quat tmp;
quat_add(&tmp, o, q);
quat_copy(o, &tmp);
}
void quat_to_axis_angle(const Quat p, Vec4f axis, float* angle) {
Quat q;
quat_copy(p, q);
quat_normalize(q, q); // if w>1 acos and sqrt will produce errors, this cant happen if quaternion is normalised
*angle = (float)(2.0 * acos(q[3]));
double s = sqrt(1.0 - q[3] * q[3]); // assuming quaternion normalised then w is less than 1, so term always positive.
if( s < CUTE_EPSILON ) { // test to avoid divide by zero, s is always positive due to sqrt
// if s close to zero then direction of axis not important
axis[0] = q[0]; // if it is important that axis is normalised then replace with x=1; y=z=0;
axis[1] = q[1];
axis[2] = q[2];
axis[3] = 1.0;
} else {
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wconversion"
#pragma warning(push)
#pragma warning(disable : 4244)
axis[0] = q[0] / s; // normalise axis
axis[1] = q[1] / s;
axis[2] = q[2] / s;
axis[3] = 1.0;
#pragma warning(pop)
#pragma GCC diagnostic pop
}
float length = vec_length(axis);
if( length < CUTE_EPSILON ) {
*angle = 0.0f;
}
}
union quat *quat_slerp(union quat *qo, const union quat *qfrom, const union quat *qto, float t)
{
/* calc cosine */
double cosom = quat_dot(qfrom, qto);
/* qto = qfrom or qto = -qfrom so no rotation to slerp */
if (cosom >= 1.0) {
quat_copy(qo, qfrom);
return qo;
}
/* adjust for shortest path */
union quat to1;
if (cosom < 0.0) {
cosom = -cosom;
to1.v.x = -qto->v.x;
to1.v.y = -qto->v.y;
to1.v.z = -qto->v.z;
to1.v.w = -qto->v.w;
} else {
quat_copy(&to1, qto);
}
/* calculate coefficients */
double scale0, scale1;
if (cosom < 0.99995) {
/* standard case (slerp) */
double omega = acos(cosom);
double sinom = sin(omega);
scale0 = sin((1.0 - t) * omega) / sinom;
scale1 = sin(t * omega) / sinom;
} else {
/* "from" and "to" quaternions are very close
* ... so we can do a linear interpolation
*/
scale0 = 1.0 - t;
scale1 = t;
}
/* calculate final values */
qo->v.x = scale0 * qfrom->v.x + scale1 * to1.v.x;
qo->v.y = scale0 * qfrom->v.y + scale1 * to1.v.y;
qo->v.z = scale0 * qfrom->v.z + scale1 * to1.v.z;
qo->v.w = scale0 * qfrom->v.w + scale1 * to1.v.w;
return qo;
}
static void quat_mulby(quat_t dst, quat_t p) /* in place, dst = dst * q */
{
quat_t q;
quat_copy(q, dst);
dst[0] = q[0]*p[0] - q[1]*p[1] - q[2]*p[2] - q[3]*p[3];
dst[1] = q[0]*p[1] + q[1]*p[0] + q[2]*p[3] - q[3]*p[2];
dst[2] = q[0]*p[2] + q[2]*p[0] + q[3]*p[1] - q[1]*p[3];
dst[3] = q[0]*p[3] + q[3]*p[0] + q[1]*p[2] - q[2]*p[1];
}
void quat_mul(AD_Quaternion *q1, AD_Quaternion *q2, AD_Quaternion *q3)
{
AD_Quaternion qtmp;
qtmp.x=q1->w*q2->x + q1->x*q2->w + q1->y*q2->z - q1->z*q2->y;
qtmp.y=q1->w*q2->y - q1->x*q2->z + q1->y*q2->w + q1->z*q2->x;
qtmp.z=q1->w*q2->z + q1->x*q2->y - q1->y*q2->x + q1->z*q2->w;
qtmp.w=q1->w*q2->w - q1->x*q2->x - q1->y*q2->y - q1->z*q2->z;
quat_copy(&qtmp, q3);
}
void quat_mul_axis_angle(const Quat q, const Vec3f axis, const float angle, Quat r) {
if( (fabs(axis[0]) < CUTE_EPSILON && fabs(axis[1]) < CUTE_EPSILON && fabs(axis[2]) < CUTE_EPSILON) ||
fabs(angle) < CUTE_EPSILON )
{
quat_copy(q, r);
}
Quat rotation = {0};
quat_from_axis_angle(axis, angle, rotation);
quat_mul(q, rotation, r);
}
int32 CGeometricObject::m_Clone(CGeometricObject *source)
{
if (!source) return(0);
strcpy(p_FatherName, source->p_FatherName);
p_Father=source->p_Father;
p_HasChildrens=source->p_HasChildrens;
p_BaseMaterial=source->p_BaseMaterial;
for (int32 i=0; i<MAX_LODS; i++)
p_Lods[i]=source->p_Lods[i];
p_NumLods=source->p_NumLods;
p_VertexBuffer=source->p_VertexBuffer;
p_StaticVertex=source->p_StaticVertex;
p_PosTrack=source->p_PosTrack;
p_RotTrack=source->p_RotTrack;
p_ScaleTrack=source->p_ScaleTrack;
vect_copy(&source->p_Pivot, &p_Pivot);
vect_copy(&source->p_CurrentPosition, &p_CurrentPosition);
quat_copy(&source->p_CurrentRotationQuaternion, &p_CurrentRotationQuaternion);
mat_copy(&source->p_CurrentRotationMatrix, &p_CurrentRotationMatrix);
vect_copy(&source->p_CurrentScale, &p_CurrentScale);
vect_copy(&source->p_TotalScale, &p_TotalScale);
mat_copy(&source->p_WorldMatrix, &p_WorldMatrix);
if (source->p_SPHEREBoundVolume)
{
p_SPHEREBoundVolume=new CBoundVolume;
source->p_SPHEREBoundVolume->m_Copy(CLONE, p_SPHEREBoundVolume);
}
if (source->p_AABBBoundVolume)
{
p_AABBBoundVolume=new CBoundVolume;
source->p_AABBBoundVolume->m_Copy(CLONE, p_AABBBoundVolume);
}
if (source->p_OBBBoundVolume)
{
p_OBBBoundVolume=new CBoundVolume;
source->p_OBBBoundVolume->m_Copy(CLONE, p_OBBBoundVolume);
}
return(1);
}
/* create quaternion that rotates a point along vector (x,y,z) by theta [rad] */
void
quat_make_from_rvec (quaternion_t result, const double theta, const double x, const double y, const double z)
{
double norm = sqrt (x*x + y*y + z*z);
double vec[3] = {x, y, z};
int i;
if (norm < QUAT_EPS)
{
quat_copy (result, quat_one);
return;
}
for (i = 0; i < 3; ++i)
{
quat_im (result, i) = sin (theta/2.0) * vec[i]/norm;
}
quat_re (result) = cos (theta/2.0);
//quat_normalize (result);
}
static void updateAttitude(AccelsData * accelsData, GyrosData * gyrosData)
{
float dT;
portTickType thisSysTime = xTaskGetTickCount();
static portTickType lastSysTime = 0;
static float accels_filtered[3] = {0,0,0};
static float grot_filtered[3] = {0,0,0};
dT = (thisSysTime == lastSysTime) ? 0.001f : TICKS2MS(portMAX_DELAY & (thisSysTime - lastSysTime)) / 1000.0f;
lastSysTime = thisSysTime;
// Bad practice to assume structure order, but saves memory
float * gyros = &gyrosData->x;
float * accels = &accelsData->x;
float grot[3];
float accel_err[3];
// Apply smoothing to accel values, to reduce vibration noise before main calculations.
apply_accel_filter(accels,accels_filtered);
// Rotate gravity to body frame, filter and cross with accels
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
// Apply same filtering to the rotated attitude to match delays
apply_accel_filter(grot,grot_filtered);
// Compute the error between the predicted direction of gravity and smoothed acceleration
CrossProduct((const float *) accels_filtered, (const float *) grot_filtered, accel_err);
// Account for accel magnitude
float accel_mag = sqrtf(accels_filtered[0]*accels_filtered[0] + accels_filtered[1]*accels_filtered[1] + accels_filtered[2]*accels_filtered[2]);
// Account for filtered gravity vector magnitude
float grot_mag;
if (accel_filter_enabled)
grot_mag = sqrtf(grot_filtered[0]*grot_filtered[0] + grot_filtered[1]*grot_filtered[1] + grot_filtered[2]*grot_filtered[2]);
else
grot_mag = 1.0f;
if (grot_mag > 1.0e-3f && accel_mag > 1.0e-3f) {
accel_err[0] /= (accel_mag*grot_mag);
accel_err[1] /= (accel_mag*grot_mag);
accel_err[2] /= (accel_mag*grot_mag);
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
gyro_correct_int[0] -= accel_err[0] * accelKi;
gyro_correct_int[1] -= accel_err[1] * accelKi;
// Correct rates based on error, integral component dealt with in updateSensors
gyros[0] += accel_err[0] * accelKp / dT;
gyros[1] += accel_err[1] * accelKp / dT;
gyros[2] += accel_err[2] * accelKp / dT;
}
{ // scoping variables to save memory
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
float qdot[4];
qdot[0] = (-q[1] * gyros[0] - q[2] * gyros[1] - q[3] * gyros[2]) * dT * DEG2RAD / 2;
qdot[1] = (q[0] * gyros[0] - q[3] * gyros[1] + q[2] * gyros[2]) * dT * DEG2RAD / 2;
qdot[2] = (q[3] * gyros[0] + q[0] * gyros[1] - q[1] * gyros[2]) * dT * DEG2RAD / 2;
qdot[3] = (-q[2] * gyros[0] + q[1] * gyros[1] + q[0] * gyros[2]) * dT * DEG2RAD / 2;
// Take a time step
q[0] = q[0] + qdot[0];
q[1] = q[1] + qdot[1];
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
if(q[0] < 0) {
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
}
// Renomalize
float qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if((fabs(qmag) < 1e-3) || (qmag != qmag)) {
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
quat_copy(q, &attitudeActual.q1);
// Convert into eueler degrees (makes assumptions about RPY order)
Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll);
AttitudeActualSet(&attitudeActual);
}
int32_t pivot_lookat(struct Pivot* pivot, const Vec3f target) {
int32_t result = -1;
Vec4f right_axis = RIGHT_AXIS;
Vec4f up_axis = UP_AXIS;
Vec4f forward_axis = FORWARD_AXIS;
struct Pivot world_pivot;
pivot_combine(pivot->parent, pivot, &world_pivot);
Vec4f target_direction;
vec_sub(target, world_pivot.position, target_direction);
float dot_yaw = vec_dot(target_direction, forward_axis);
Quat rotation = {0};
if( fabs(dot_yaw + 1.0f) < CUTE_EPSILON ) {
// vector a and b point exactly in the opposite direction,
// so it is a 180 degrees turn around the up-axis
Quat rotation = {0};
quat_from_axis_angle(up_axis, PI, rotation);
quat_mul(world_pivot.orientation, rotation, rotation);
} else if( fabs(dot_yaw - (1.0f)) < CUTE_EPSILON ) {
// vector a and b point exactly in the same direction
// so we return the identity quaternion
quat_copy(world_pivot.orientation, rotation);
} else {
// - I look at the target by turning the pivot orientation using only
// yaw and pitch movement
// - I always rotate the pivot from its initial orientation (up and forward
// basis vectors like initialized above), so this does not incrementally
// advance the orientation
quat_identity(rotation);
// - to find the amount of yaw I project the target_direction into the
// up_axis plane, resulting in up_projection which is a vector that
// points from the up_axis plane to the tip of the target_direction
Vec4f up_projection = {0};
vec_mul1f(up_axis, vec_dot(target_direction, up_axis), up_projection);
// - so then by subtracting the up_projection from the target_direction,
// I get a vector lying in the up_axis plane, pointing towards the target
Vec4f yaw_direction = {0};
vec_sub(target_direction, up_projection, yaw_direction);
// - angle between yaw_direction and forward_axis is the amount of yaw we
// need to point the forward_axis toward the target
float yaw = 0.0f;
vec_angle(yaw_direction, forward_axis, &yaw);
log_assert( ! isnan(yaw),
"vec_angle(%f %f %f, %f %f %f, %f);\n",
yaw_direction[0], yaw_direction[1], yaw_direction[2],
forward_axis[0], forward_axis[1], forward_axis[2],
yaw );
// - I have to compute the cross product between yaw_direction and
// forward_axis and use the resulting yaw_axis
Vec4f yaw_axis = {0};
vec_cross(yaw_direction, forward_axis, yaw_axis);
if( vec_nullp(yaw_axis) ) {
vec_copy4f(up_axis, yaw_axis);
}
// - compute the yaw rotation
Quat yaw_rotation = {0};
quat_from_axis_angle(yaw_axis, yaw, yaw_rotation);
// - to compute, just as with the yaw, I want an axis that lies on the plane that
// is spanned in this case by the right_axis, when the camera points toward the
// target
// - I could compute an axis, but I already have a direction vector that points
// toward the target, the yaw_direction, I just have to normalize it to make it
// an axis (and put the result in forward_axis, since it now is the forward_axis
// of the yaw turned camera)
Vec4f yaw_forward_axis = {0};
vec_normalize(yaw_direction, yaw_forward_axis);
// - then use the new forward axis with the old target_direction to compute the angle
// between those
float pitch = 0.0f;
vec_angle(target_direction, yaw_forward_axis, &pitch);
log_assert( ! isnan(pitch),
"vec_angle(%f %f %f, %f %f %f, %f);\n",
target_direction[0], target_direction[1], target_direction[2],
yaw_forward_axis[0], yaw_forward_axis[1], yaw_forward_axis[2],
pitch );
// - and just as in the yaw case we compute an rotation pitch_axis
Vec4f pitch_axis = {0};
vec_cross(target_direction, yaw_forward_axis, pitch_axis);
if( vec_nullp(pitch_axis) ) {
vec_copy4f(right_axis, pitch_axis);
}
// - and finally compute the pitch rotation and combine it with the yaw_rotation
// in the same step
Quat pitch_rotation;
quat_from_axis_angle(pitch_axis, pitch, pitch_rotation);
Quat yaw_pitch_rotation;
quat_mul(yaw_rotation, pitch_rotation, yaw_pitch_rotation);
Quat inverted_orientation = {0};
quat_invert(world_pivot.orientation, inverted_orientation);
// - the int32_t I want to return indicates the cameras 'flip' status, that is, it is
// one when the camera angle was pitched so much that it flipped over and its
// up axis is now pointing downwards
// - to find out if I am flipped over, I compute the flipped up_axis called
// flip_axis and then use the dot product between the flip_axis and up_axis
// to decide if I am flipped
Vec4f flip_axis = {0};
vec_rotate(up_axis, inverted_orientation, flip_axis);
vec_rotate(flip_axis, yaw_pitch_rotation, flip_axis);
float dot_pitch = vec_dot(up_axis, flip_axis);
Vec4f target_axis = {0};
vec_normalize(target_direction, target_axis);
// - check if we are flipped and if we are, set result to 1 meaning we are flipped
// - turn the camera around PI so that we can continue pitching, otherwise we just get
// stuck when trying to flip the camera over
if( dot_pitch < 0.0f ) {
result = 1;
quat_from_axis_angle(target_axis, PI, rotation);
quat_mul(yaw_pitch_rotation, rotation, yaw_pitch_rotation);
}
quat_copy(yaw_pitch_rotation, rotation);
}
if( ! isnan(rotation[0]) &&
! isnan(rotation[1]) &&
! isnan(rotation[2]) &&
! isnan(rotation[3]) )
{
quat_copy(rotation, pivot->orientation);
}
pivot->eye_distance = vec_length(target_direction);
return result;
}
qboolean R_LoadMD5(model_t *mod, void *buffer, int bufferSize, const char *modName)
{
int i, j, k;
md5Model_t *md5;
md5Bone_t *bone;
md5Surface_t *surf;
srfTriangle_t *tri;
md5Vertex_t *v;
md5Weight_t *weight;
int version;
shader_t *sh;
char *buf_p = ( char * ) buffer;
char *token;
vec3_t boneOrigin;
quat_t boneQuat;
mat4_t boneMat;
int numRemaining;
growList_t sortedTriangles;
growList_t vboTriangles;
growList_t vboSurfaces;
int numBoneReferences;
int boneReferences[MAX_BONES];
// skip MD5Version indent string
(void) COM_ParseExt2(&buf_p, qfalse);
// check version
token = COM_ParseExt2(&buf_p, qfalse);
version = atoi(token);
if (version != MD5_VERSION)
{
Ren_Warning("R_LoadMD5: %s has wrong version (%i should be %i)\n", modName, version, MD5_VERSION);
return qfalse;
}
mod->type = MOD_MD5;
mod->dataSize += sizeof(md5Model_t);
md5 = mod->md5 = ri.Hunk_Alloc(sizeof(md5Model_t), h_low);
// skip commandline <arguments string>
token = COM_ParseExt2(&buf_p, qtrue);
token = COM_ParseExt2(&buf_p, qtrue);
// Ren_Print("%s\n", token);
// parse numJoints <number>
token = COM_ParseExt2(&buf_p, qtrue);
if (Q_stricmp(token, "numJoints"))
{
Ren_Warning("R_LoadMD5: expected 'numJoints' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
token = COM_ParseExt2(&buf_p, qfalse);
md5->numBones = atoi(token);
// parse numMeshes <number>
token = COM_ParseExt2(&buf_p, qtrue);
if (Q_stricmp(token, "numMeshes"))
{
Ren_Warning("R_LoadMD5: expected 'numMeshes' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
token = COM_ParseExt2(&buf_p, qfalse);
md5->numSurfaces = atoi(token);
//Ren_Print("R_LoadMD5: '%s' has %i surfaces\n", modName, md5->numSurfaces);
if (md5->numBones < 1)
{
Ren_Warning("R_LoadMD5: '%s' has no bones\n", modName);
return qfalse;
}
if (md5->numBones > MAX_BONES)
{
Ren_Warning("R_LoadMD5: '%s' has more than %i bones (%i)\n", modName, MAX_BONES, md5->numBones);
return qfalse;
}
//Ren_Print("R_LoadMD5: '%s' has %i bones\n", modName, md5->numBones);
// parse all the bones
md5->bones = ri.Hunk_Alloc(sizeof(*bone) * md5->numBones, h_low);
// parse joints {
token = COM_ParseExt2(&buf_p, qtrue);
if (Q_stricmp(token, "joints"))
{
Ren_Warning("R_LoadMD5: expected 'joints' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
token = COM_ParseExt2(&buf_p, qfalse);
if (Q_stricmp(token, "{"))
{
Ren_Warning("R_LoadMD5: expected '{' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
for (i = 0, bone = md5->bones; i < md5->numBones; i++, bone++)
{
token = COM_ParseExt2(&buf_p, qtrue);
Q_strncpyz(bone->name, token, sizeof(bone->name));
//Ren_Print("R_LoadMD5: '%s' has bone '%s'\n", modName, bone->name);
token = COM_ParseExt2(&buf_p, qfalse);
bone->parentIndex = atoi(token);
//Ren_Print("R_LoadMD5: '%s' has bone '%s' with parent index %i\n", modName, bone->name, bone->parentIndex);
if (bone->parentIndex >= md5->numBones)
{
Ren_Drop("R_LoadMD5: '%s' has bone '%s' with bad parent index %i while numBones is %i", modName,
bone->name, bone->parentIndex, md5->numBones);
}
// skip (
token = COM_ParseExt2(&buf_p, qfalse);
if (Q_stricmp(token, "("))
{
Ren_Warning("R_LoadMD5: expected '(' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
for (j = 0; j < 3; j++)
{
token = COM_ParseExt2(&buf_p, qfalse);
boneOrigin[j] = atof(token);
}
// skip )
token = COM_ParseExt2(&buf_p, qfalse);
if (Q_stricmp(token, ")"))
{
Ren_Warning("R_LoadMD5: expected ')' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
// skip (
token = COM_ParseExt2(&buf_p, qfalse);
if (Q_stricmp(token, "("))
{
Ren_Warning("R_LoadMD5: expected '(' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
for (j = 0; j < 3; j++)
{
token = COM_ParseExt2(&buf_p, qfalse);
boneQuat[j] = atof(token);
}
QuatCalcW(boneQuat);
mat4_from_quat(boneMat, boneQuat);
VectorCopy(boneOrigin, bone->origin);
quat_copy(boneQuat, bone->rotation);
MatrixSetupTransformFromQuat(bone->inverseTransform, boneQuat, boneOrigin);
mat4_inverse_self(bone->inverseTransform);
// skip )
token = COM_ParseExt2(&buf_p, qfalse);
if (Q_stricmp(token, ")"))
{
Ren_Warning("R_LoadMD5: expected '(' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
}
// parse }
token = COM_ParseExt2(&buf_p, qtrue);
if (Q_stricmp(token, "}"))
{
Ren_Warning("R_LoadMD5: expected '}' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
// parse all the surfaces
if (md5->numSurfaces < 1)
{
Ren_Warning("R_LoadMD5: '%s' has no surfaces\n", modName);
return qfalse;
}
//Ren_Print("R_LoadMD5: '%s' has %i surfaces\n", modName, md5->numSurfaces);
md5->surfaces = ri.Hunk_Alloc(sizeof(*surf) * md5->numSurfaces, h_low);
for (i = 0, surf = md5->surfaces; i < md5->numSurfaces; i++, surf++)
{
// parse mesh {
token = COM_ParseExt2(&buf_p, qtrue);
if (Q_stricmp(token, "mesh"))
{
Ren_Warning("R_LoadMD5: expected 'mesh' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
token = COM_ParseExt2(&buf_p, qfalse);
if (Q_stricmp(token, "{"))
{
Ren_Warning("R_LoadMD5: expected '{' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
// change to surface identifier
surf->surfaceType = SF_MD5;
// give pointer to model for Tess_SurfaceMD5
surf->model = md5;
// parse shader <name>
token = COM_ParseExt2(&buf_p, qtrue);
if (Q_stricmp(token, "shader"))
{
Ren_Warning("R_LoadMD5: expected 'shader' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
token = COM_ParseExt2(&buf_p, qfalse);
Q_strncpyz(surf->shader, token, sizeof(surf->shader));
//Ren_Print("R_LoadMD5: '%s' uses shader '%s'\n", modName, surf->shader);
// FIXME .md5mesh meshes don't have surface names
// lowercase the surface name so skin compares are faster
//Q_strlwr(surf->name);
//Ren_Print("R_LoadMD5: '%s' has surface '%s'\n", modName, surf->name);
// register the shaders
sh = R_FindShader(surf->shader, SHADER_3D_DYNAMIC, qtrue);
if (sh->defaultShader)
{
surf->shaderIndex = 0;
}
else
{
surf->shaderIndex = sh->index;
}
// parse numVerts <number>
token = COM_ParseExt2(&buf_p, qtrue);
if (Q_stricmp(token, "numVerts"))
{
Ren_Warning("R_LoadMD5: expected 'numVerts' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
token = COM_ParseExt2(&buf_p, qfalse);
surf->numVerts = atoi(token);
if (surf->numVerts > SHADER_MAX_VERTEXES)
{
Ren_Drop("R_LoadMD5: '%s' has more than %i verts on a surface (%i)",
modName, SHADER_MAX_VERTEXES, surf->numVerts);
}
surf->verts = ri.Hunk_Alloc(sizeof(*v) * surf->numVerts, h_low);
for (j = 0, v = surf->verts; j < surf->numVerts; j++, v++)
{
// skip vert <number>
token = COM_ParseExt2(&buf_p, qtrue);
if (Q_stricmp(token, "vert"))
{
Ren_Warning("R_LoadMD5: expected 'vert' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
(void) COM_ParseExt2(&buf_p, qfalse);
// skip (
token = COM_ParseExt2(&buf_p, qfalse);
if (Q_stricmp(token, "("))
{
Ren_Warning("R_LoadMD5: expected '(' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
for (k = 0; k < 2; k++)
{
token = COM_ParseExt2(&buf_p, qfalse);
v->texCoords[k] = atof(token);
}
// skip )
token = COM_ParseExt2(&buf_p, qfalse);
if (Q_stricmp(token, ")"))
{
Ren_Warning("R_LoadMD5: expected ')' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
token = COM_ParseExt2(&buf_p, qfalse);
v->firstWeight = atoi(token);
token = COM_ParseExt2(&buf_p, qfalse);
v->numWeights = atoi(token);
if (v->numWeights > MAX_WEIGHTS)
{
Ren_Drop("R_LoadMD5: vertex %i requires more than %i weights on surface (%i) in model '%s'",
j, MAX_WEIGHTS, i, modName);
}
}
// parse numTris <number>
token = COM_ParseExt2(&buf_p, qtrue);
if (Q_stricmp(token, "numTris"))
{
Ren_Warning("R_LoadMD5: expected 'numTris' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
token = COM_ParseExt2(&buf_p, qfalse);
surf->numTriangles = atoi(token);
if (surf->numTriangles > SHADER_MAX_TRIANGLES)
{
Ren_Drop("R_LoadMD5: '%s' has more than %i triangles on a surface (%i)",
modName, SHADER_MAX_TRIANGLES, surf->numTriangles);
}
surf->triangles = ri.Hunk_Alloc(sizeof(*tri) * surf->numTriangles, h_low);
for (j = 0, tri = surf->triangles; j < surf->numTriangles; j++, tri++)
{
// skip tri <number>
token = COM_ParseExt2(&buf_p, qtrue);
if (Q_stricmp(token, "tri"))
{
Ren_Warning("R_LoadMD5: expected 'tri' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
(void) COM_ParseExt2(&buf_p, qfalse);
for (k = 0; k < 3; k++)
{
token = COM_ParseExt2(&buf_p, qfalse);
tri->indexes[k] = atoi(token);
}
}
// parse numWeights <number>
token = COM_ParseExt2(&buf_p, qtrue);
if (Q_stricmp(token, "numWeights"))
{
Ren_Warning("R_LoadMD5: expected 'numWeights' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
token = COM_ParseExt2(&buf_p, qfalse);
surf->numWeights = atoi(token);
surf->weights = ri.Hunk_Alloc(sizeof(*weight) * surf->numWeights, h_low);
for (j = 0, weight = surf->weights; j < surf->numWeights; j++, weight++)
{
// skip weight <number>
token = COM_ParseExt2(&buf_p, qtrue);
if (Q_stricmp(token, "weight"))
{
Ren_Warning("R_LoadMD5: expected 'weight' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
(void) COM_ParseExt2(&buf_p, qfalse);
token = COM_ParseExt2(&buf_p, qfalse);
weight->boneIndex = atoi(token);
token = COM_ParseExt2(&buf_p, qfalse);
weight->boneWeight = atof(token);
// skip (
token = COM_ParseExt2(&buf_p, qfalse);
if (Q_stricmp(token, "("))
{
Ren_Warning("R_LoadMD5: expected '(' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
for (k = 0; k < 3; k++)
{
token = COM_ParseExt2(&buf_p, qfalse);
weight->offset[k] = atof(token);
}
// skip )
token = COM_ParseExt2(&buf_p, qfalse);
if (Q_stricmp(token, ")"))
{
Ren_Warning("R_LoadMD5: expected ')' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
}
// parse }
token = COM_ParseExt2(&buf_p, qtrue);
if (Q_stricmp(token, "}"))
{
Ren_Warning("R_LoadMD5: expected '}' found '%s' in model '%s'\n", token, modName);
return qfalse;
}
// loop trough all vertices and set up the vertex weights
for (j = 0, v = surf->verts; j < surf->numVerts; j++, v++)
{
v->weights = ri.Hunk_Alloc(sizeof(*v->weights) * v->numWeights, h_low);
for (k = 0; k < v->numWeights; k++)
{
v->weights[k] = surf->weights + (v->firstWeight + k);
}
}
}
// loading is done now calculate the bounding box and tangent spaces
ClearBounds(md5->bounds[0], md5->bounds[1]);
for (i = 0, surf = md5->surfaces; i < md5->numSurfaces; i++, surf++)
{
for (j = 0, v = surf->verts; j < surf->numVerts; j++, v++)
{
vec3_t tmpVert;
md5Weight_t *w;
VectorClear(tmpVert);
for (k = 0, w = v->weights[0]; k < v->numWeights; k++, w++)
{
vec3_t offsetVec;
bone = &md5->bones[w->boneIndex];
QuatTransformVector(bone->rotation, w->offset, offsetVec);
VectorAdd(bone->origin, offsetVec, offsetVec);
VectorMA(tmpVert, w->boneWeight, offsetVec, tmpVert);
}
VectorCopy(tmpVert, v->position);
AddPointToBounds(tmpVert, md5->bounds[0], md5->bounds[1]);
}
// calc tangent spaces
#if 1
{
const float *v0, *v1, *v2;
const float *t0, *t1, *t2;
vec3_t tangent;
vec3_t binormal;
vec3_t normal;
for (j = 0, v = surf->verts; j < surf->numVerts; j++, v++)
{
VectorClear(v->tangent);
VectorClear(v->binormal);
VectorClear(v->normal);
}
for (j = 0, tri = surf->triangles; j < surf->numTriangles; j++, tri++)
{
v0 = surf->verts[tri->indexes[0]].position;
v1 = surf->verts[tri->indexes[1]].position;
v2 = surf->verts[tri->indexes[2]].position;
t0 = surf->verts[tri->indexes[0]].texCoords;
t1 = surf->verts[tri->indexes[1]].texCoords;
t2 = surf->verts[tri->indexes[2]].texCoords;
#if 1
R_CalcTangentSpace(tangent, binormal, normal, v0, v1, v2, t0, t1, t2);
#else
R_CalcNormalForTriangle(normal, v0, v1, v2);
R_CalcTangentsForTriangle(tangent, binormal, v0, v1, v2, t0, t1, t2);
#endif
for (k = 0; k < 3; k++)
{
float *v;
v = surf->verts[tri->indexes[k]].tangent;
VectorAdd(v, tangent, v);
v = surf->verts[tri->indexes[k]].binormal;
VectorAdd(v, binormal, v);
v = surf->verts[tri->indexes[k]].normal;
VectorAdd(v, normal, v);
}
}
for (j = 0, v = surf->verts; j < surf->numVerts; j++, v++)
{
VectorNormalize(v->tangent);
VectorNormalize(v->binormal);
VectorNormalize(v->normal);
}
}
#else
{
int k;
float bb, s, t;
vec3_t bary;
vec3_t faceNormal;
md5Vertex_t *dv[3];
for (j = 0, tri = surf->triangles; j < surf->numTriangles; j++, tri++)
{
dv[0] = &surf->verts[tri->indexes[0]];
dv[1] = &surf->verts[tri->indexes[1]];
dv[2] = &surf->verts[tri->indexes[2]];
R_CalcNormalForTriangle(faceNormal, dv[0]->position, dv[1]->position, dv[2]->position);
// calculate barycentric basis for the triangle
bb = (dv[1]->texCoords[0] - dv[0]->texCoords[0]) * (dv[2]->texCoords[1] - dv[0]->texCoords[1]) - (dv[2]->texCoords[0] - dv[0]->texCoords[0]) * (dv[1]->texCoords[1] -
dv[0]->texCoords[1]);
if (fabs(bb) < 0.00000001f)
{
continue;
}
// do each vertex
for (k = 0; k < 3; k++)
{
// calculate s tangent vector
s = dv[k]->texCoords[0] + 10.0f;
t = dv[k]->texCoords[1];
bary[0] = ((dv[1]->texCoords[0] - s) * (dv[2]->texCoords[1] - t) - (dv[2]->texCoords[0] - s) * (dv[1]->texCoords[1] - t)) / bb;
bary[1] = ((dv[2]->texCoords[0] - s) * (dv[0]->texCoords[1] - t) - (dv[0]->texCoords[0] - s) * (dv[2]->texCoords[1] - t)) / bb;
bary[2] = ((dv[0]->texCoords[0] - s) * (dv[1]->texCoords[1] - t) - (dv[1]->texCoords[0] - s) * (dv[0]->texCoords[1] - t)) / bb;
dv[k]->tangent[0] = bary[0] * dv[0]->position[0] + bary[1] * dv[1]->position[0] + bary[2] * dv[2]->position[0];
dv[k]->tangent[1] = bary[0] * dv[0]->position[1] + bary[1] * dv[1]->position[1] + bary[2] * dv[2]->position[1];
dv[k]->tangent[2] = bary[0] * dv[0]->position[2] + bary[1] * dv[1]->position[2] + bary[2] * dv[2]->position[2];
VectorSubtract(dv[k]->tangent, dv[k]->position, dv[k]->tangent);
VectorNormalize(dv[k]->tangent);
// calculate t tangent vector (binormal)
s = dv[k]->texCoords[0];
t = dv[k]->texCoords[1] + 10.0f;
bary[0] = ((dv[1]->texCoords[0] - s) * (dv[2]->texCoords[1] - t) - (dv[2]->texCoords[0] - s) * (dv[1]->texCoords[1] - t)) / bb;
bary[1] = ((dv[2]->texCoords[0] - s) * (dv[0]->texCoords[1] - t) - (dv[0]->texCoords[0] - s) * (dv[2]->texCoords[1] - t)) / bb;
bary[2] = ((dv[0]->texCoords[0] - s) * (dv[1]->texCoords[1] - t) - (dv[1]->texCoords[0] - s) * (dv[0]->texCoords[1] - t)) / bb;
dv[k]->binormal[0] = bary[0] * dv[0]->position[0] + bary[1] * dv[1]->position[0] + bary[2] * dv[2]->position[0];
dv[k]->binormal[1] = bary[0] * dv[0]->position[1] + bary[1] * dv[1]->position[1] + bary[2] * dv[2]->position[1];
dv[k]->binormal[2] = bary[0] * dv[0]->position[2] + bary[1] * dv[1]->position[2] + bary[2] * dv[2]->position[2];
VectorSubtract(dv[k]->binormal, dv[k]->position, dv[k]->binormal);
VectorNormalize(dv[k]->binormal);
// calculate the normal as cross product N=TxB
#if 0
CrossProduct(dv[k]->tangent, dv[k]->binormal, dv[k]->normal);
VectorNormalize(dv[k]->normal);
// Gram-Schmidt orthogonalization process for B
// compute the cross product B=NxT to obtain
// an orthogonal basis
CrossProduct(dv[k]->normal, dv[k]->tangent, dv[k]->binormal);
if (DotProduct(dv[k]->normal, faceNormal) < 0)
{
VectorInverse(dv[k]->normal);
//VectorInverse(dv[k]->tangent);
//VectorInverse(dv[k]->binormal);
}
#else
VectorAdd(dv[k]->normal, faceNormal, dv[k]->normal);
#endif
}
}
#if 1
for (j = 0, v = surf->verts; j < surf->numVerts; j++, v++)
{
//VectorNormalize(v->tangent);
//VectorNormalize(v->binormal);
VectorNormalize(v->normal);
}
#endif
}
#endif
#if 0
// do another extra smoothing for normals to avoid flat shading
for (j = 0; j < surf->numVerts; j++)
{
for (k = 0; k < surf->numVerts; k++)
{
if (j == k)
{
continue;
}
if (VectorCompare(surf->verts[j].position, surf->verts[k].position))
{
VectorAdd(surf->verts[j].normal, surf->verts[k].normal, surf->verts[j].normal);
}
}
VectorNormalize(surf->verts[j].normal);
}
#endif
}
// split the surfaces into VBO surfaces by the maximum number of GPU vertex skinning bones
Com_InitGrowList(&vboSurfaces, 10);
for (i = 0, surf = md5->surfaces; i < md5->numSurfaces; i++, surf++)
{
// sort triangles
Com_InitGrowList(&sortedTriangles, 1000);
for (j = 0, tri = surf->triangles; j < surf->numTriangles; j++, tri++)
{
skelTriangle_t *sortTri = Com_Allocate(sizeof(*sortTri));
for (k = 0; k < 3; k++)
{
sortTri->indexes[k] = tri->indexes[k];
sortTri->vertexes[k] = &surf->verts[tri->indexes[k]];
}
sortTri->referenced = qfalse;
Com_AddToGrowList(&sortedTriangles, sortTri);
}
//qsort(sortedTriangles.elements, sortedTriangles.currentElements, sizeof(void *), CompareTrianglesByBoneReferences);
#if 0
for (j = 0; j < sortedTriangles.currentElements; j++)
{
int b[MAX_WEIGHTS * 3];
skelTriangle_t *sortTri = Com_GrowListElement(&sortedTriangles, j);
for (k = 0; k < 3; k++)
{
v = sortTri->vertexes[k];
for (l = 0; l < MAX_WEIGHTS; l++)
{
b[k * 3 + l] = (l < v->numWeights) ? v->weights[l]->boneIndex : 9999;
}
qsort(b, MAX_WEIGHTS * 3, sizeof(int), CompareBoneIndices);
//Ren_Print("bone indices: %i %i %i %i\n", b[k * 3 + 0], b[k * 3 + 1], b[k * 3 + 2], b[k * 3 + 3]);
}
}
#endif
numRemaining = sortedTriangles.currentElements;
while (numRemaining)
{
numBoneReferences = 0;
Com_Memset(boneReferences, 0, sizeof(boneReferences));
Com_InitGrowList(&vboTriangles, 1000);
for (j = 0; j < sortedTriangles.currentElements; j++)
{
skelTriangle_t *sortTri = Com_GrowListElement(&sortedTriangles, j);
if (sortTri->referenced)
{
continue;
}
if (AddTriangleToVBOTriangleList(&vboTriangles, sortTri, &numBoneReferences, boneReferences))
{
sortTri->referenced = qtrue;
}
}
if (!vboTriangles.currentElements)
{
Ren_Warning("R_LoadMD5: could not add triangles to a remaining VBO surfaces for model '%s'\n", modName);
Com_DestroyGrowList(&vboTriangles);
break;
}
AddSurfaceToVBOSurfacesList(&vboSurfaces, &vboTriangles, md5, surf, i, numBoneReferences, boneReferences);
numRemaining -= vboTriangles.currentElements;
Com_DestroyGrowList(&vboTriangles);
}
for (j = 0; j < sortedTriangles.currentElements; j++)
{
skelTriangle_t *sortTri = Com_GrowListElement(&sortedTriangles, j);
Com_Dealloc(sortTri);
}
Com_DestroyGrowList(&sortedTriangles);
}
// move VBO surfaces list to hunk
md5->numVBOSurfaces = vboSurfaces.currentElements;
md5->vboSurfaces = ri.Hunk_Alloc(md5->numVBOSurfaces * sizeof(*md5->vboSurfaces), h_low);
for (i = 0; i < md5->numVBOSurfaces; i++)
{
md5->vboSurfaces[i] = ( srfVBOMD5Mesh_t * ) Com_GrowListElement(&vboSurfaces, i);
}
Com_DestroyGrowList(&vboSurfaces);
return qtrue;
}
qboolean R_LoadPSK(model_t *mod, void *buffer, int bufferSize, const char *modName)
{
int i, j, k;
memStream_t *stream = NULL;
axChunkHeader_t chunkHeader;
int numPoints;
axPoint_t *point;
axPoint_t *points = NULL;
int numVertexes;
axVertex_t *vertex;
axVertex_t *vertexes = NULL;
//int numSmoothGroups;
int numTriangles;
axTriangle_t *triangle;
axTriangle_t *triangles = NULL;
int numMaterials;
axMaterial_t *material;
axMaterial_t *materials = NULL;
int numReferenceBones;
axReferenceBone_t *refBone;
axReferenceBone_t *refBones = NULL;
int numWeights;
axBoneWeight_t *axWeight;
axBoneWeight_t *axWeights = NULL;
md5Model_t *md5;
md5Bone_t *md5Bone;
md5Weight_t *weight;
vec3_t boneOrigin;
quat_t boneQuat;
//mat4_t boneMat;
int materialIndex, oldMaterialIndex;
int numRemaining;
growList_t sortedTriangles;
growList_t vboVertexes;
growList_t vboTriangles;
growList_t vboSurfaces;
int numBoneReferences;
int boneReferences[MAX_BONES];
mat4_t unrealToQuake;
#define DeallocAll() Com_Dealloc(materials); \
Com_Dealloc(points); \
Com_Dealloc(vertexes); \
Com_Dealloc(triangles); \
Com_Dealloc(refBones); \
Com_Dealloc(axWeights); \
FreeMemStream(stream);
//MatrixSetupScale(unrealToQuake, 1, -1, 1);
mat4_from_angles(unrealToQuake, 0, 90, 0);
stream = AllocMemStream(buffer, bufferSize);
GetChunkHeader(stream, &chunkHeader);
// check indent again
if (Q_stricmpn(chunkHeader.ident, "ACTRHEAD", 8))
{
Ren_Warning("R_LoadPSK: '%s' has wrong chunk indent ('%s' should be '%s')\n", modName, chunkHeader.ident, "ACTRHEAD");
DeallocAll();
return qfalse;
}
PrintChunkHeader(&chunkHeader);
mod->type = MOD_MD5;
mod->dataSize += sizeof(md5Model_t);
md5 = mod->md5 = ri.Hunk_Alloc(sizeof(md5Model_t), h_low);
// read points
GetChunkHeader(stream, &chunkHeader);
if (Q_stricmpn(chunkHeader.ident, "PNTS0000", 8))
{
Ren_Warning("R_LoadPSK: '%s' has wrong chunk indent ('%s' should be '%s')\n", modName, chunkHeader.ident, "PNTS0000");
DeallocAll();
return qfalse;
}
if (chunkHeader.dataSize != sizeof(axPoint_t))
{
Ren_Warning("R_LoadPSK: '%s' has wrong chunk dataSize ('%i' should be '%i')\n", modName, chunkHeader.dataSize, ( int ) sizeof(axPoint_t));
DeallocAll();
return qfalse;
}
PrintChunkHeader(&chunkHeader);
numPoints = chunkHeader.numData;
points = Com_Allocate(numPoints * sizeof(axPoint_t));
for (i = 0, point = points; i < numPoints; i++, point++)
{
point->point[0] = MemStreamGetFloat(stream);
point->point[1] = MemStreamGetFloat(stream);
point->point[2] = MemStreamGetFloat(stream);
#if 0
// HACK convert from Unreal coordinate system to the Quake one
MatrixTransformPoint2(unrealToQuake, point->point);
#endif
}
// read vertices
GetChunkHeader(stream, &chunkHeader);
if (Q_stricmpn(chunkHeader.ident, "VTXW0000", 8))
{
Ren_Warning("R_LoadPSK: '%s' has wrong chunk indent ('%s' should be '%s')\n", modName, chunkHeader.ident, "VTXW0000");
DeallocAll();
return qfalse;
}
if (chunkHeader.dataSize != sizeof(axVertex_t))
{
Ren_Warning("R_LoadPSK: '%s' has wrong chunk dataSize ('%i' should be '%i')\n", modName, chunkHeader.dataSize, ( int ) sizeof(axVertex_t));
DeallocAll();
return qfalse;
}
PrintChunkHeader(&chunkHeader);
numVertexes = chunkHeader.numData;
vertexes = Com_Allocate(numVertexes * sizeof(axVertex_t));
{
int tmpVertexInt = -1; // tmp vertex member values - MemStreamGet functions return -1 if they fail
// now we print a warning if they do or abort if pointIndex is invalid
for (i = 0, vertex = vertexes; i < numVertexes; i++, vertex++)
{
tmpVertexInt = MemStreamGetShort(stream);
if (tmpVertexInt < 0 || tmpVertexInt >= numPoints)
{
ri.Printf(PRINT_ERROR, "R_LoadPSK: '%s' has vertex with point index out of range (%i while max %i)\n", modName, tmpVertexInt, numPoints);
DeallocAll();
return qfalse;
}
vertex->pointIndex = tmpVertexInt;
tmpVertexInt = MemStreamGetShort(stream);
if (tmpVertexInt < 0)
{
Ren_Warning("R_LoadPSK: MemStream NULL or empty (vertex->unknownA)\n");
}
vertex->unknownA = tmpVertexInt;
vertex->st[0] = MemStreamGetFloat(stream);
if (vertex->st[0] == -1)
{
Ren_Warning("R_LoadPSK: MemStream possibly NULL or empty (vertex->st[0])\n");
}
vertex->st[1] = MemStreamGetFloat(stream);
if (vertex->st[1] == -1)
{
Ren_Warning("R_LoadPSK: MemStream possibly NULL or empty (vertex->st[1])\n");
}
tmpVertexInt = MemStreamGetC(stream);
if (tmpVertexInt < 0)
{
Ren_Warning("R_LoadPSK: MemStream NULL or empty (vertex->materialIndex)\n");
}
vertex->materialIndex = tmpVertexInt;
tmpVertexInt = MemStreamGetC(stream);
if (tmpVertexInt < 0)
{
Ren_Warning("R_LoadPSK: MemStream NULL or empty (vertex->materialIndex)\n");
}
vertex->reserved = tmpVertexInt;
tmpVertexInt = MemStreamGetShort(stream);
if (tmpVertexInt < 0)
{
Ren_Warning("R_LoadPSK: MemStream NULL or empty (vertex->materialIndex)\n");
}
vertex->unknownB = tmpVertexInt;
#if 0
Ren_Print("R_LoadPSK: axVertex_t(%i):\n"
"axVertex:pointIndex: %i\n"
"axVertex:unknownA: %i\n"
"axVertex::st: %f %f\n"
"axVertex:materialIndex: %i\n"
"axVertex:reserved: %d\n"
"axVertex:unknownB: %d\n",
i,
vertex->pointIndex,
vertex->unknownA,
vertex->st[0], vertex->st[1],
vertex->materialIndex,
vertex->reserved,
vertex->unknownB);
#endif
}
// read triangles
GetChunkHeader(stream, &chunkHeader);
if (Q_stricmpn(chunkHeader.ident, "FACE0000", 8))
{
Ren_Warning("R_LoadPSK: '%s' has wrong chunk indent ('%s' should be '%s')\n", modName, chunkHeader.ident, "FACE0000");
DeallocAll();
return qfalse;
}
if (chunkHeader.dataSize != sizeof(axTriangle_t))
{
Ren_Warning("R_LoadPSK: '%s' has wrong chunk dataSize ('%i' should be '%i')\n", modName, chunkHeader.dataSize, ( int ) sizeof(axTriangle_t));
DeallocAll();
return qfalse;
}
PrintChunkHeader(&chunkHeader);
numTriangles = chunkHeader.numData;
triangles = Com_Allocate(numTriangles * sizeof(axTriangle_t));
for (i = 0, triangle = triangles; i < numTriangles; i++, triangle++)
{
for (j = 0; j < 3; j++)
//for(j = 2; j >= 0; j--)
{
tmpVertexInt = MemStreamGetShort(stream);
if (tmpVertexInt < 0)
{
Ren_Warning("R_LoadPSK: '%s' MemStream NULL or empty (triangle->indexes[%i])\n", modName, j);
DeallocAll();
return qfalse;
}
if (tmpVertexInt >= numVertexes)
{
Ren_Warning("R_LoadPSK: '%s' has triangle with vertex index out of range (%i while max %i)\n", modName, tmpVertexInt, numVertexes);
DeallocAll();
return qfalse;
}
triangle->indexes[j] = tmpVertexInt;
}
triangle->materialIndex = MemStreamGetC(stream);
triangle->materialIndex2 = MemStreamGetC(stream);
triangle->smoothingGroups = MemStreamGetLong(stream);
}
}
// read materials
GetChunkHeader(stream, &chunkHeader);
if (Q_stricmpn(chunkHeader.ident, "MATT0000", 8))
{
Ren_Warning("R_LoadPSK: '%s' has wrong chunk indent ('%s' should be '%s')\n", modName, chunkHeader.ident, "MATT0000");
DeallocAll();
return qfalse;
}
if (chunkHeader.dataSize != sizeof(axMaterial_t))
{
Ren_Warning("R_LoadPSK: '%s' has wrong chunk dataSize ('%i' should be '%i')\n", modName, chunkHeader.dataSize, ( int ) sizeof(axMaterial_t));
DeallocAll();
return qfalse;
}
PrintChunkHeader(&chunkHeader);
numMaterials = chunkHeader.numData;
materials = Com_Allocate(numMaterials * sizeof(axMaterial_t));
for (i = 0, material = materials; i < numMaterials; i++, material++)
{
MemStreamRead(stream, material->name, sizeof(material->name));
Ren_Print("R_LoadPSK: material name: '%s'\n", material->name);
material->shaderIndex = MemStreamGetLong(stream);
material->polyFlags = MemStreamGetLong(stream);
material->auxMaterial = MemStreamGetLong(stream);
material->auxFlags = MemStreamGetLong(stream);
material->lodBias = MemStreamGetLong(stream);
material->lodStyle = MemStreamGetLong(stream);
}
for (i = 0, vertex = vertexes; i < numVertexes; i++, vertex++)
{
if (vertex->materialIndex < 0 || vertex->materialIndex >= numMaterials)
{
Ren_Warning("R_LoadPSK: '%s' has vertex with material index out of range (%i while max %i)\n", modName, vertex->materialIndex, numMaterials);
DeallocAll();
return qfalse;
}
}
for (i = 0, triangle = triangles; i < numTriangles; i++, triangle++)
{
if (triangle->materialIndex < 0 || triangle->materialIndex >= numMaterials)
{
Ren_Warning("R_LoadPSK: '%s' has triangle with material index out of range (%i while max %i)\n", modName, triangle->materialIndex, numMaterials);
DeallocAll();
return qfalse;
}
}
// read reference bones
GetChunkHeader(stream, &chunkHeader);
if (Q_stricmpn(chunkHeader.ident, "REFSKELT", 8))
{
Ren_Warning("R_LoadPSK: '%s' has wrong chunk indent ('%s' should be '%s')\n", modName, chunkHeader.ident, "REFSKELT");
DeallocAll();
return qfalse;
}
if (chunkHeader.dataSize != sizeof(axReferenceBone_t))
{
Ren_Warning("R_LoadPSK: '%s' has wrong chunk dataSize ('%i' should be '%i')\n", modName, chunkHeader.dataSize, ( int ) sizeof(axReferenceBone_t));
DeallocAll();
return qfalse;
}
PrintChunkHeader(&chunkHeader);
numReferenceBones = chunkHeader.numData;
refBones = Com_Allocate(numReferenceBones * sizeof(axReferenceBone_t));
for (i = 0, refBone = refBones; i < numReferenceBones; i++, refBone++)
{
MemStreamRead(stream, refBone->name, sizeof(refBone->name));
//Ren_Print("R_LoadPSK: reference bone name: '%s'\n", refBone->name);
refBone->flags = MemStreamGetLong(stream);
refBone->numChildren = MemStreamGetLong(stream);
refBone->parentIndex = MemStreamGetLong(stream);
GetBone(stream, &refBone->bone);
#if 0
Ren_Print("R_LoadPSK: axReferenceBone_t(%i):\n"
"axReferenceBone_t::name: '%s'\n"
"axReferenceBone_t::flags: %i\n"
"axReferenceBone_t::numChildren %i\n"
"axReferenceBone_t::parentIndex: %i\n"
"axReferenceBone_t::quat: %f %f %f %f\n"
"axReferenceBone_t::position: %f %f %f\n"
"axReferenceBone_t::length: %f\n"
"axReferenceBone_t::xSize: %f\n"
"axReferenceBone_t::ySize: %f\n"
"axReferenceBone_t::zSize: %f\n",
i,
refBone->name,
refBone->flags,
refBone->numChildren,
refBone->parentIndex,
refBone->bone.quat[0], refBone->bone.quat[1], refBone->bone.quat[2], refBone->bone.quat[3],
refBone->bone.position[0], refBone->bone.position[1], refBone->bone.position[2],
refBone->bone.length,
refBone->bone.xSize,
refBone->bone.ySize,
refBone->bone.zSize);
#endif
}
// read bone weights
GetChunkHeader(stream, &chunkHeader);
if (Q_stricmpn(chunkHeader.ident, "RAWWEIGHTS", 10))
{
Ren_Warning("R_LoadPSK: '%s' has wrong chunk indent ('%s' should be '%s')\n", modName, chunkHeader.ident, "RAWWEIGHTS");
DeallocAll();
return qfalse;
}
if (chunkHeader.dataSize != sizeof(axBoneWeight_t))
{
Ren_Warning("R_LoadPSK: '%s' has wrong chunk dataSize ('%i' should be '%i')\n", modName, chunkHeader.dataSize, ( int ) sizeof(axBoneWeight_t));
DeallocAll();
return qfalse;
}
PrintChunkHeader(&chunkHeader);
numWeights = chunkHeader.numData;
axWeights = Com_Allocate(numWeights * sizeof(axBoneWeight_t));
for (i = 0, axWeight = axWeights; i < numWeights; i++, axWeight++)
{
axWeight->weight = MemStreamGetFloat(stream);
axWeight->pointIndex = MemStreamGetLong(stream);
axWeight->boneIndex = MemStreamGetLong(stream);
#if 0
Ren_Print("R_LoadPSK: axBoneWeight_t(%i):\n"
"axBoneWeight_t::weight: %f\n"
"axBoneWeight_t::pointIndex %i\n"
"axBoneWeight_t::boneIndex: %i\n",
i,
axWeight->weight,
axWeight->pointIndex,
axWeight->boneIndex);
#endif
}
//
// convert the model to an internal MD5 representation
//
md5->numBones = numReferenceBones;
// calc numMeshes <number>
/*
numSmoothGroups = 0;
for(i = 0, triangle = triangles; i < numTriangles; i++, triangle++)
{
if(triangle->smoothingGroups)
{
}
}
*/
if (md5->numBones < 1)
{
Ren_Warning("R_LoadPSK: '%s' has no bones\n", modName);
DeallocAll();
return qfalse;
}
if (md5->numBones > MAX_BONES)
{
Ren_Warning("R_LoadPSK: '%s' has more than %i bones (%i)\n", modName, MAX_BONES, md5->numBones);
DeallocAll();
return qfalse;
}
//Ren_Print("R_LoadPSK: '%s' has %i bones\n", modName, md5->numBones);
// copy all reference bones
md5->bones = ri.Hunk_Alloc(sizeof(*md5Bone) * md5->numBones, h_low);
for (i = 0, md5Bone = md5->bones, refBone = refBones; i < md5->numBones; i++, md5Bone++, refBone++)
{
Q_strncpyz(md5Bone->name, refBone->name, sizeof(md5Bone->name));
if (i == 0)
{
md5Bone->parentIndex = refBone->parentIndex - 1;
}
else
{
md5Bone->parentIndex = refBone->parentIndex;
}
//Ren_Print("R_LoadPSK: '%s' has bone '%s' with parent index %i\n", modName, md5Bone->name, md5Bone->parentIndex);
if (md5Bone->parentIndex >= md5->numBones)
{
DeallocAll();
Ren_Drop("R_LoadPSK: '%s' has bone '%s' with bad parent index %i while numBones is %i", modName,
md5Bone->name, md5Bone->parentIndex, md5->numBones);
}
for (j = 0; j < 3; j++)
{
boneOrigin[j] = refBone->bone.position[j];
}
// I have really no idea why the .psk format stores the first quaternion with inverted quats.
// Furthermore only the X and Z components of the first quat are inverted ?!?!
if (i == 0)
{
boneQuat[0] = refBone->bone.quat[0];
boneQuat[1] = -refBone->bone.quat[1];
boneQuat[2] = refBone->bone.quat[2];
boneQuat[3] = refBone->bone.quat[3];
}
else
{
boneQuat[0] = -refBone->bone.quat[0];
boneQuat[1] = -refBone->bone.quat[1];
boneQuat[2] = -refBone->bone.quat[2];
boneQuat[3] = refBone->bone.quat[3];
}
VectorCopy(boneOrigin, md5Bone->origin);
//MatrixTransformPoint(unrealToQuake, boneOrigin, md5Bone->origin);
quat_copy(boneQuat, md5Bone->rotation);
//QuatClear(md5Bone->rotation);
#if 0
Ren_Print("R_LoadPSK: md5Bone_t(%i):\n"
"md5Bone_t::name: '%s'\n"
"md5Bone_t::parentIndex: %i\n"
"md5Bone_t::quat: %f %f %f %f\n"
"md5bone_t::position: %f %f %f\n",
i,
md5Bone->name,
md5Bone->parentIndex,
md5Bone->rotation[0], md5Bone->rotation[1], md5Bone->rotation[2], md5Bone->rotation[3],
md5Bone->origin[0], md5Bone->origin[1], md5Bone->origin[2]);
#endif
if (md5Bone->parentIndex >= 0)
{
vec3_t rotated;
quat_t quat;
md5Bone_t *parent;
parent = &md5->bones[md5Bone->parentIndex];
QuatTransformVector(parent->rotation, md5Bone->origin, rotated);
//QuatTransformVector(md5Bone->rotation, md5Bone->origin, rotated);
VectorAdd(parent->origin, rotated, md5Bone->origin);
QuatMultiply1(parent->rotation, md5Bone->rotation, quat);
quat_copy(quat, md5Bone->rotation);
}
MatrixSetupTransformFromQuat(md5Bone->inverseTransform, md5Bone->rotation, md5Bone->origin);
mat4_inverse_self(md5Bone->inverseTransform);
#if 0
Ren_Print("R_LoadPSK: md5Bone_t(%i):\n"
"md5Bone_t::name: '%s'\n"
"md5Bone_t::parentIndex: %i\n"
"md5Bone_t::quat: %f %f %f %f\n"
"md5bone_t::position: %f %f %f\n",
i,
md5Bone->name,
md5Bone->parentIndex,
md5Bone->rotation[0], md5Bone->rotation[1], md5Bone->rotation[2], md5Bone->rotation[3],
md5Bone->origin[0], md5Bone->origin[1], md5Bone->origin[2]);
#endif
}
Com_InitGrowList(&vboVertexes, 10000);
for (i = 0, vertex = vertexes; i < numVertexes; i++, vertex++)
{
md5Vertex_t *vboVert = Com_Allocate(sizeof(*vboVert));
for (j = 0; j < 3; j++)
{
vboVert->position[j] = points[vertex->pointIndex].point[j];
}
vboVert->texCoords[0] = vertex->st[0];
vboVert->texCoords[1] = vertex->st[1];
// find number of associated weights
vboVert->numWeights = 0;
for (j = 0, axWeight = axWeights; j < numWeights; j++, axWeight++)
{
if (axWeight->pointIndex == vertex->pointIndex && axWeight->weight > 0.0f)
{
vboVert->numWeights++;
}
}
if (vboVert->numWeights > MAX_WEIGHTS)
{
DeallocAll();
Ren_Drop("R_LoadPSK: vertex %i requires more weights %i than the maximum of %i in model '%s'", i, vboVert->numWeights, MAX_WEIGHTS, modName);
//Ren_Warning( "R_LoadPSK: vertex %i requires more weights %i than the maximum of %i in model '%s'\n", i, vboVert->numWeights, MAX_WEIGHTS, modName);
}
vboVert->weights = ri.Hunk_Alloc(sizeof(*vboVert->weights) * vboVert->numWeights, h_low);
for (j = 0, axWeight = axWeights, k = 0; j < numWeights; j++, axWeight++)
{
if (axWeight->pointIndex == vertex->pointIndex && axWeight->weight > 0.0f)
{
weight = ri.Hunk_Alloc(sizeof(*weight), h_low);
weight->boneIndex = axWeight->boneIndex;
weight->boneWeight = axWeight->weight;
// FIXME?
weight->offset[0] = refBones[axWeight->boneIndex].bone.xSize;
weight->offset[1] = refBones[axWeight->boneIndex].bone.ySize;
weight->offset[2] = refBones[axWeight->boneIndex].bone.zSize;
vboVert->weights[k++] = weight;
}
}
Com_AddToGrowList(&vboVertexes, vboVert);
}
ClearBounds(md5->bounds[0], md5->bounds[1]);
for (i = 0, vertex = vertexes; i < numVertexes; i++, vertex++)
{
AddPointToBounds(points[vertex->pointIndex].point, md5->bounds[0], md5->bounds[1]);
}
#if 0
Ren_Print("R_LoadPSK: AABB (%i %i %i) (%i %i %i)\n",
( int ) md5->bounds[0][0],
( int ) md5->bounds[0][1],
( int ) md5->bounds[0][2],
( int ) md5->bounds[1][0],
( int ) md5->bounds[1][1],
( int ) md5->bounds[1][2]);
#endif
// sort triangles
qsort(triangles, numTriangles, sizeof(axTriangle_t), CompareTrianglesByMaterialIndex);
Com_InitGrowList(&sortedTriangles, 1000);
for (i = 0, triangle = triangles; i < numTriangles; i++, triangle++)
{
skelTriangle_t *sortTri = Com_Allocate(sizeof(*sortTri));
for (j = 0; j < 3; j++)
{
sortTri->indexes[j] = triangle->indexes[j];
sortTri->vertexes[j] = Com_GrowListElement(&vboVertexes, triangle->indexes[j]);
}
sortTri->referenced = qfalse;
Com_AddToGrowList(&sortedTriangles, sortTri);
}
// calc tangent spaces
#if 1
{
md5Vertex_t *v0, *v1, *v2;
const float *p0, *p1, *p2;
const float *t0, *t1, *t2;
vec3_t tangent = { 0, 0, 0 };
vec3_t binormal;
vec3_t normal;
for (j = 0; j < vboVertexes.currentElements; j++)
{
v0 = Com_GrowListElement(&vboVertexes, j);
VectorClear(v0->tangent);
VectorClear(v0->binormal);
VectorClear(v0->normal);
}
for (j = 0; j < sortedTriangles.currentElements; j++)
{
skelTriangle_t *tri = Com_GrowListElement(&sortedTriangles, j);
v0 = Com_GrowListElement(&vboVertexes, tri->indexes[0]);
v1 = Com_GrowListElement(&vboVertexes, tri->indexes[1]);
v2 = Com_GrowListElement(&vboVertexes, tri->indexes[2]);
p0 = v0->position;
p1 = v1->position;
p2 = v2->position;
t0 = v0->texCoords;
t1 = v1->texCoords;
t2 = v2->texCoords;
#if 1
R_CalcTangentSpace(tangent, binormal, normal, p0, p1, p2, t0, t1, t2);
#else
R_CalcNormalForTriangle(normal, p0, p1, p2);
R_CalcTangentsForTriangle(tangent, binormal, p0, p1, p2, t0, t1, t2);
#endif
for (k = 0; k < 3; k++)
{
float *v;
v0 = Com_GrowListElement(&vboVertexes, tri->indexes[k]);
v = v0->tangent;
VectorAdd(v, tangent, v);
v = v0->binormal;
VectorAdd(v, binormal, v);
v = v0->normal;
VectorAdd(v, normal, v);
}
}
for (j = 0; j < vboVertexes.currentElements; j++)
{
v0 = Com_GrowListElement(&vboVertexes, j);
VectorNormalize(v0->tangent);
VectorNormalize(v0->binormal);
VectorNormalize(v0->normal);
}
}
#else
{
float bb, s, t;
vec3_t bary;
vec3_t faceNormal;
md5Vertex_t *dv[3];
for (j = 0; j < sortedTriangles.currentElements; j++)
{
skelTriangle_t *tri = Com_GrowListElement(&sortedTriangles, j);
dv[0] = Com_GrowListElement(&vboVertexes, tri->indexes[0]);
dv[1] = Com_GrowListElement(&vboVertexes, tri->indexes[1]);
dv[2] = Com_GrowListElement(&vboVertexes, tri->indexes[2]);
R_CalcNormalForTriangle(faceNormal, dv[0]->position, dv[1]->position, dv[2]->position);
// calculate barycentric basis for the triangle
bb = (dv[1]->texCoords[0] - dv[0]->texCoords[0]) * (dv[2]->texCoords[1] - dv[0]->texCoords[1]) - (dv[2]->texCoords[0] - dv[0]->texCoords[0]) * (dv[1]->texCoords[1] -
dv[0]->texCoords[1]);
if (fabs(bb) < 0.00000001f)
{
continue;
}
// do each vertex
for (k = 0; k < 3; k++)
{
// calculate s tangent vector
s = dv[k]->texCoords[0] + 10.0f;
t = dv[k]->texCoords[1];
bary[0] = ((dv[1]->texCoords[0] - s) * (dv[2]->texCoords[1] - t) - (dv[2]->texCoords[0] - s) * (dv[1]->texCoords[1] - t)) / bb;
bary[1] = ((dv[2]->texCoords[0] - s) * (dv[0]->texCoords[1] - t) - (dv[0]->texCoords[0] - s) * (dv[2]->texCoords[1] - t)) / bb;
bary[2] = ((dv[0]->texCoords[0] - s) * (dv[1]->texCoords[1] - t) - (dv[1]->texCoords[0] - s) * (dv[0]->texCoords[1] - t)) / bb;
dv[k]->tangent[0] = bary[0] * dv[0]->position[0] + bary[1] * dv[1]->position[0] + bary[2] * dv[2]->position[0];
dv[k]->tangent[1] = bary[0] * dv[0]->position[1] + bary[1] * dv[1]->position[1] + bary[2] * dv[2]->position[1];
dv[k]->tangent[2] = bary[0] * dv[0]->position[2] + bary[1] * dv[1]->position[2] + bary[2] * dv[2]->position[2];
VectorSubtract(dv[k]->tangent, dv[k]->position, dv[k]->tangent);
VectorNormalize(dv[k]->tangent);
// calculate t tangent vector (binormal)
s = dv[k]->texCoords[0];
t = dv[k]->texCoords[1] + 10.0f;
bary[0] = ((dv[1]->texCoords[0] - s) * (dv[2]->texCoords[1] - t) - (dv[2]->texCoords[0] - s) * (dv[1]->texCoords[1] - t)) / bb;
bary[1] = ((dv[2]->texCoords[0] - s) * (dv[0]->texCoords[1] - t) - (dv[0]->texCoords[0] - s) * (dv[2]->texCoords[1] - t)) / bb;
bary[2] = ((dv[0]->texCoords[0] - s) * (dv[1]->texCoords[1] - t) - (dv[1]->texCoords[0] - s) * (dv[0]->texCoords[1] - t)) / bb;
dv[k]->binormal[0] = bary[0] * dv[0]->position[0] + bary[1] * dv[1]->position[0] + bary[2] * dv[2]->position[0];
dv[k]->binormal[1] = bary[0] * dv[0]->position[1] + bary[1] * dv[1]->position[1] + bary[2] * dv[2]->position[1];
dv[k]->binormal[2] = bary[0] * dv[0]->position[2] + bary[1] * dv[1]->position[2] + bary[2] * dv[2]->position[2];
VectorSubtract(dv[k]->binormal, dv[k]->position, dv[k]->binormal);
VectorNormalize(dv[k]->binormal);
// calculate the normal as cross product N=TxB
#if 0
CrossProduct(dv[k]->tangent, dv[k]->binormal, dv[k]->normal);
VectorNormalize(dv[k]->normal);
// Gram-Schmidt orthogonalization process for B
// compute the cross product B=NxT to obtain
// an orthogonal basis
CrossProduct(dv[k]->normal, dv[k]->tangent, dv[k]->binormal);
if (DotProduct(dv[k]->normal, faceNormal) < 0)
{
VectorInverse(dv[k]->normal);
//VectorInverse(dv[k]->tangent);
//VectorInverse(dv[k]->binormal);
}
#else
VectorAdd(dv[k]->normal, faceNormal, dv[k]->normal);
#endif
}
}
#if 1
for (j = 0; j < vboVertexes.currentElements; j++)
{
dv[0] = Com_GrowListElement(&vboVertexes, j);
//VectorNormalize(dv[0]->tangent);
//VectorNormalize(dv[0]->binormal);
VectorNormalize(dv[0]->normal);
}
#endif
}
#endif
#if 0
{
md5Vertex_t *v0, *v1;
// do another extra smoothing for normals to avoid flat shading
for (j = 0; j < vboVertexes.currentElements; j++)
{
v0 = Com_GrowListElement(&vboVertexes, j);
for (k = 0; k < vboVertexes.currentElements; k++)
{
if (j == k)
{
continue;
}
v1 = Com_GrowListElement(&vboVertexes, k);
if (VectorCompare(v0->position, v1->position))
{
VectorAdd(v0->position, v1->normal, v0->normal);
}
}
VectorNormalize(v0->normal);
}
}
#endif
// split the surfaces into VBO surfaces by the maximum number of GPU vertex skinning bones
Com_InitGrowList(&vboSurfaces, 10);
materialIndex = oldMaterialIndex = -1;
for (i = 0; i < numTriangles; i++)
{
triangle = &triangles[i];
materialIndex = triangle->materialIndex;
if (materialIndex != oldMaterialIndex)
{
oldMaterialIndex = materialIndex;
numRemaining = sortedTriangles.currentElements - i;
while (numRemaining)
{
numBoneReferences = 0;
Com_Memset(boneReferences, 0, sizeof(boneReferences));
Com_InitGrowList(&vboTriangles, 1000);
for (j = i; j < sortedTriangles.currentElements; j++)
{
skelTriangle_t *sortTri;
triangle = &triangles[j];
materialIndex = triangle->materialIndex;
if (materialIndex != oldMaterialIndex)
{
continue;
}
sortTri = Com_GrowListElement(&sortedTriangles, j);
if (sortTri->referenced)
{
continue;
}
if (AddTriangleToVBOTriangleList(&vboTriangles, sortTri, &numBoneReferences, boneReferences))
{
sortTri->referenced = qtrue;
}
}
for (j = 0; j < MAX_BONES; j++)
{
if (boneReferences[j] > 0)
{
Ren_Print("R_LoadPSK: referenced bone: '%s'\n", (j < numReferenceBones) ? refBones[j].name : NULL);
}
}
if (!vboTriangles.currentElements)
{
Ren_Warning("R_LoadPSK: could not add triangles to a remaining VBO surface for model '%s'\n", modName);
break;
}
// FIXME skinIndex
AddSurfaceToVBOSurfacesList2(&vboSurfaces, &vboTriangles, &vboVertexes, md5, vboSurfaces.currentElements, materials[oldMaterialIndex].name, numBoneReferences, boneReferences);
numRemaining -= vboTriangles.currentElements;
Com_DestroyGrowList(&vboTriangles);
}
}
}
for (j = 0; j < sortedTriangles.currentElements; j++)
{
skelTriangle_t *sortTri = Com_GrowListElement(&sortedTriangles, j);
Com_Dealloc(sortTri);
}
Com_DestroyGrowList(&sortedTriangles);
for (j = 0; j < vboVertexes.currentElements; j++)
{
md5Vertex_t *v = Com_GrowListElement(&vboVertexes, j);
Com_Dealloc(v);
}
Com_DestroyGrowList(&vboVertexes);
// move VBO surfaces list to hunk
md5->numVBOSurfaces = vboSurfaces.currentElements;
md5->vboSurfaces = ri.Hunk_Alloc(md5->numVBOSurfaces * sizeof(*md5->vboSurfaces), h_low);
for (i = 0; i < md5->numVBOSurfaces; i++)
{
md5->vboSurfaces[i] = ( srfVBOMD5Mesh_t * ) Com_GrowListElement(&vboSurfaces, i);
}
Com_DestroyGrowList(&vboSurfaces);
FreeMemStream(stream);
Com_Dealloc(points);
Com_Dealloc(vertexes);
Com_Dealloc(triangles);
Com_Dealloc(materials);
Ren_Developer("%i VBO surfaces created for PSK model '%s'\n", md5->numVBOSurfaces, modName);
return qtrue;
}
int main(void)
{
i2c_bus_t bus;
int ret = i2c_bus_open(&bus, "/dev/i2c-3");
if (ret < 0)
{
fatal("could not open i2c bus", ret);
return EXIT_FAILURE;
}
/* ITG: */
itg3200_dev_t itg;
itg_again:
ret = itg3200_init(&itg, &bus, ITG3200_DLPF_42HZ);
if (ret < 0)
{
fatal("could not inizialize ITG3200", ret);
if (ret == -EAGAIN)
{
goto itg_again;
}
return EXIT_FAILURE;
}
/* BMA: */
bma180_dev_t bma;
bma180_init(&bma, &bus, BMA180_RANGE_4G, BMA180_BW_10HZ);
/* HMC: */
hmc5883_dev_t hmc;
hmc5883_init(&hmc, &bus);
/* MS: */
ms5611_dev_t ms;
ret = ms5611_init(&ms, &bus, MS5611_OSR4096, MS5611_OSR4096);
if (ret < 0)
{
fatal("could not inizialize MS5611", ret);
return EXIT_FAILURE;
}
pthread_t thread;
pthread_create(&thread, NULL, ms5611_reader, &ms);
/* initialize AHRS filter: */
madgwick_ahrs_t madgwick_ahrs;
madgwick_ahrs_init(&madgwick_ahrs, STANDARD_BETA);
interval_t interval;
interval_init(&interval);
float init = START_BETA;
udp_socket_t *socket = udp_socket_create("10.0.0.100", 5005, 0, 0);
/* kalman filter: */
kalman_t kalman1, kalman2, kalman3;
kalman_init(&kalman1, 1.0e-6, 1.0e-2, 0, 0);
kalman_init(&kalman2, 1.0e-6, 1.0e-2, 0, 0);
kalman_init(&kalman3, 1.0e-6, 1.0e-2, 0, 0);
vec3_t global_acc; /* x = N, y = E, z = D */
int init_done = 0;
int converged = 0;
sliding_avg_t *avg[3];
avg[0] = sliding_avg_create(1000, 0.0);
avg[1] = sliding_avg_create(1000, 0.0);
avg[2] = sliding_avg_create(1000, -9.81);
float alt_rel_last = 0.0;
int udp_cnt = 0;
while (1)
{
int i;
float dt = interval_measure(&interval);
init -= BETA_STEP;
if (init < FINAL_BETA)
{
init = FINAL_BETA;
init_done = 1;
}
madgwick_ahrs.beta = init;
/* sensor data acquisition: */
itg3200_read_gyro(&itg);
bma180_read_acc(&bma);
hmc5883_read(&hmc);
/* state estimates and output: */
euler_t euler;
madgwick_ahrs_update(&madgwick_ahrs, itg.gyro.x, itg.gyro.y, itg.gyro.z, bma.raw.x, bma.raw.y, bma.raw.z, hmc.raw.x, hmc.raw.y, hmc.raw.z, 11.0, dt);
quat_t q_body_to_world;
quat_copy(&q_body_to_world, &madgwick_ahrs.quat);
quat_rot_vec(&global_acc, &bma.raw, &q_body_to_world);
for (i = 0; i < 3; i++)
{
global_acc.vec[i] -= sliding_avg_calc(avg[i], global_acc.vec[i]);
}
if (init_done)
{
kalman_in_t kalman_in;
kalman_in.dt = dt;
kalman_in.pos = 0;
kalman_out_t kalman_out;
kalman_in.acc = global_acc.x;
kalman_run(&kalman_out, &kalman1, &kalman_in);
kalman_in.acc = global_acc.y;
kalman_run(&kalman_out, &kalman2, &kalman_in);
kalman_in.acc = -global_acc.z;
pthread_mutex_lock(&mutex);
kalman_in.pos = alt_rel;
pthread_mutex_unlock(&mutex);
kalman_run(&kalman_out, &kalman3, &kalman_in);
if (!converged)
{
if (fabs(kalman_out.pos - alt_rel) < 0.1)
{
converged = 1;
fprintf(stderr, "init done\n");
}
}
if (converged) // && udp_cnt++ > 10)
{
if (udp_cnt++ == 10)
{
char buffer[1024];
udp_cnt = 0;
int len = sprintf(buffer, "%f %f %f %f %f %f %f", madgwick_ahrs.quat.q0, madgwick_ahrs.quat.q1, madgwick_ahrs.quat.q2, madgwick_ahrs.quat.q3,
global_acc.x, global_acc.y, global_acc.z);
udp_socket_send(socket, buffer, len);
}
printf("%f %f %f\n", -global_acc.z, alt_rel, kalman_out.pos);
fflush(stdout);
}
}
}
return 0;
}
static int32_t updateAttitudeComplementary(bool first_run)
{
UAVObjEvent ev;
GyrosData gyrosData;
AccelsData accelsData;
static int32_t timeval;
float dT;
static uint8_t init = 0;
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ||
xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE )
{
// When one of these is updated so should the other
// Do not set attitude timeout warnings in simulation mode
if (!AttitudeActualReadOnly()){
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
return -1;
}
}
AccelsGet(&accelsData);
// During initialization and
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(first_run) {
#if defined(PIOS_INCLUDE_HMC5883)
// To initialize we need a valid mag reading
if ( xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE )
return -1;
MagnetometerData magData;
MagnetometerGet(&magData);
#else
MagnetometerData magData;
magData.x = 100;
magData.y = 0;
magData.z = 0;
#endif
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
init = 0;
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
timeval = PIOS_DELAY_GetRaw();
return 0;
}
if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias
attitudeSettings.AccelKp = 1;
attitudeSettings.AccelKi = 0.9;
attitudeSettings.YawBiasRate = 0.23;
magKp = 1;
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
attitudeSettings.AccelKp = 1;
attitudeSettings.AccelKi = 0.9;
attitudeSettings.YawBiasRate = 0.23;
magKp = 1;
init = 0;
} else if (init == 0) {
// Reload settings (all the rates)
AttitudeSettingsGet(&attitudeSettings);
magKp = 0.01f;
init = 1;
}
GyrosGet(&gyrosData);
// Compute the dT using the cpu clock
dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f;
timeval = PIOS_DELAY_GetRaw();
float q[4];
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
float grot[3];
float accel_err[3];
// Get the current attitude estimate
quat_copy(&attitudeActual.q1, q);
// Rotate gravity to body frame and cross with accels
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
CrossProduct((const float *) &accelsData.x, (const float *) grot, accel_err);
// Account for accel magnitude
accel_mag = accelsData.x*accelsData.x + accelsData.y*accelsData.y + accelsData.z*accelsData.z;
accel_mag = sqrtf(accel_mag);
accel_err[0] /= accel_mag;
accel_err[1] /= accel_mag;
accel_err[2] /= accel_mag;
if ( xQueueReceive(magQueue, &ev, 0) != pdTRUE )
{
// Rotate gravity to body frame and cross with accels
float brot[3];
float Rbe[3][3];
MagnetometerData mag;
Quaternion2R(q, Rbe);
MagnetometerGet(&mag);
// If the mag is producing bad data don't use it (normally bad calibration)
if (mag.x == mag.x && mag.y == mag.y && mag.z == mag.z) {
rot_mult(Rbe, homeLocation.Be, brot);
float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
mag.x /= mag_len;
mag.y /= mag_len;
mag.z /= mag_len;
float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]);
brot[0] /= bmag;
brot[1] /= bmag;
brot[2] /= bmag;
// Only compute if neither vector is null
if (bmag < 1 || mag_len < 1)
mag_err[0] = mag_err[1] = mag_err[2] = 0;
else
CrossProduct((const float *) &mag.x, (const float *) brot, mag_err);
}
} else {
mag_err[0] = mag_err[1] = mag_err[2] = 0;
}
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi;
gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi;
gyrosBias.z -= mag_err[2] * magKi;
GyrosBiasSet(&gyrosBias);
// Correct rates based on error, integral component dealt with in updateSensors
gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT;
gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT;
gyrosData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT;
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
float qdot[4];
qdot[0] = (-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT * F_PI / 180 / 2;
qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * F_PI / 180 / 2;
qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * F_PI / 180 / 2;
qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * F_PI / 180 / 2;
// Take a time step
q[0] = q[0] + qdot[0];
q[1] = q[1] + qdot[1];
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
if(q[0] < 0) {
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
// Renomalize
qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if((fabs(qmag) < 1.0e-3f) || (qmag != qmag)) {
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
quat_copy(q, &attitudeActual.q1);
// Convert into eueler degrees (makes assumptions about RPY order)
Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll);
AttitudeActualSet(&attitudeActual);
// Flush these queues for avoid errors
xQueueReceive(baroQueue, &ev, 0);
if ( xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE ) {
float NED[3];
// Transform the GPS position into NED coordinates
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
getNED(&gpsPosition, NED);
PositionActualData positionActual;
PositionActualGet(&positionActual);
positionActual.North = NED[0];
positionActual.East = NED[1];
positionActual.Down = NED[2];
PositionActualSet(&positionActual);
}
if ( xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE ) {
// Transform the GPS position into NED coordinates
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
velocityActual.North = gpsVelocity.North;
velocityActual.East = gpsVelocity.East;
velocityActual.Down = gpsVelocity.Down;
VelocityActualSet(&velocityActual);
}
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
return 0;
}
/*
==============
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 = skelAnim->md5;
md5Channel_t *channel;
md5Frame_t *newFrame, *oldFrame;
vec3_t newOrigin, oldOrigin, lerpedOrigin;
quat_t newQuat, oldQuat, lerpedQuat;
int componentsApplied;
// 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))
//{
// Ren_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);
quat_copy(channel->baseQuat, newQuat);
quat_copy(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);
quat_norm(oldQuat);
QuatCalcW(newQuat);
quat_norm(newQuat);
#if 1
VectorLerp(oldOrigin, newOrigin, frac, lerpedOrigin);
quat_slerp(oldQuat, newQuat, frac, lerpedQuat);
#else
VectorCopy(newOrigin, lerpedOrigin);
quat_copy(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);
}
quat_copy(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 = skelAnim->psa;
axAnimationKey_t *newKey, *oldKey;
axReferenceBone_t *refBone;
vec3_t newOrigin, oldOrigin, lerpedOrigin;
quat_t newQuat, oldQuat, lerpedQuat;
refSkeleton_t skeleton;
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);
quat_copy(newKey->quat, newQuat);
quat_copy(oldKey->quat, oldQuat);
//QuatCalcW(oldQuat);
//QuatNormalize(oldQuat);
//QuatCalcW(newQuat);
//QuatNormalize(newQuat);
VectorLerp(oldOrigin, newOrigin, frac, lerpedOrigin);
quat_slerp(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);
}
quat_copy(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);
quat_copy(skel->bones[i].rotation, skeleton.bones[i].rotation);
if (refBone->parentIndex >= 0)
{
vec3_t rotated;
quat_t quat;
refBone_t *bone = &skeleton.bones[i];
refBone_t *parent = &skeleton.bones[refBone->parentIndex];
QuatTransformVector(parent->rotation, bone->origin, rotated);
VectorAdd(parent->origin, rotated, bone->origin);
QuatMultiply1(parent->rotation, bone->rotation, quat);
quat_copy(quat, bone->rotation);
AddPointToBounds(bone->origin, skel->bounds[0], skel->bounds[1]);
}
}
skel->numBones = anim->info.numBones;
skel->type = SK_RELATIVE;
return qtrue;
}
//Ren_Warning( "RE_BuildSkeleton: bad animation '%s' with handle %i\n", anim->name, hAnim);
// FIXME: clear existing bones and bounds?
return qfalse;
}
static void updateAttitude(AttitudeRawData * attitudeRaw)
{
float dT;
portTickType thisSysTime = xTaskGetTickCount();
static portTickType lastSysTime = 0;
dT = (thisSysTime == lastSysTime) ? 0.001 : (portMAX_DELAY & (thisSysTime - lastSysTime)) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
// Bad practice to assume structure order, but saves memory
float gyro[3];
gyro[0] = attitudeRaw->gyros[0];
gyro[1] = attitudeRaw->gyros[1];
gyro[2] = attitudeRaw->gyros[2];
{
float * accels = attitudeRaw->accels;
float grot[3];
float accel_err[3];
// Rotate gravity to body frame and cross with accels
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
CrossProduct((const float *) accels, (const float *) grot, accel_err);
// Account for accel magnitude
float accel_mag = sqrt(accels[0]*accels[0] + accels[1]*accels[1] + accels[2]*accels[2]);
accel_err[0] /= accel_mag;
accel_err[1] /= accel_mag;
accel_err[2] /= accel_mag;
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
gyro_correct_int[0] += accel_err[0] * accelKi;
gyro_correct_int[1] += accel_err[1] * accelKi;
//gyro_correct_int[2] += accel_err[2] * settings.AccelKI * dT;
// Correct rates based on error, integral component dealt with in updateSensors
gyro[0] += accel_err[0] * accelKp / dT;
gyro[1] += accel_err[1] * accelKp / dT;
gyro[2] += accel_err[2] * accelKp / dT;
}
{ // scoping variables to save memory
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
float qdot[4];
qdot[0] = (-q[1] * gyro[0] - q[2] * gyro[1] - q[3] * gyro[2]) * dT * M_PI / 180 / 2;
qdot[1] = (q[0] * gyro[0] - q[3] * gyro[1] + q[2] * gyro[2]) * dT * M_PI / 180 / 2;
qdot[2] = (q[3] * gyro[0] + q[0] * gyro[1] - q[1] * gyro[2]) * dT * M_PI / 180 / 2;
qdot[3] = (-q[2] * gyro[0] + q[1] * gyro[1] + q[0] * gyro[2]) * dT * M_PI / 180 / 2;
// Take a time step
q[0] = q[0] + qdot[0];
q[1] = q[1] + qdot[1];
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
}
// Renomalize
float qmag = sqrt(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if((fabs(qmag) < 1e-3) || (qmag != qmag)) {
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
quat_copy(q, &attitudeActual.q1);
// Convert into eueler degrees (makes assumptions about RPY order)
Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll);
AttitudeActualSet(&attitudeActual);
}
