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); }