From 46ba5f0023c2b907e250ea9c6b82094b70e5ebb3 Mon Sep 17 00:00:00 2001 From: Justin Marshall Date: Sun, 3 May 2026 13:46:58 -0700 Subject: [PATCH] 2007 idPhysics upgrades. --- neo/cm/CollisionModel_translate.cpp | 522 +-- neo/game/physics/Physics_AF.cpp | 4415 ++++++++++++------------ neo/game/physics/Physics_Monster.cpp | 596 ++-- neo/game/physics/Physics_Player.cpp | 1156 ++++--- neo/game/physics/Physics_RigidBody.cpp | 802 +++-- neo/idlib/bv/Bounds.h | 6 + 6 files changed, 3951 insertions(+), 3546 deletions(-) diff --git a/neo/cm/CollisionModel_translate.cpp b/neo/cm/CollisionModel_translate.cpp index 3ba94057..b75d05d0 100644 --- a/neo/cm/CollisionModel_translate.cpp +++ b/neo/cm/CollisionModel_translate.cpp @@ -2,9 +2,9 @@ =========================================================================== Doom 3 GPL Source Code -Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. +Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. -This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?). +This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?). Doom 3 Source Code is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -54,7 +54,7 @@ idCollisionModelManagerLocal::TranslateEdgeThroughEdge calculates fraction of the translation completed at which the edges collide ================ */ -ID_INLINE int idCollisionModelManagerLocal::TranslateEdgeThroughEdge( idVec3 &cross, idPluecker &l1, idPluecker &l2, float *fraction ) { +ID_INLINE int idCollisionModelManagerLocal::TranslateEdgeThroughEdge(idVec3& cross, idPluecker& l1, idPluecker& l2, float* fraction) { float d, t; @@ -159,15 +159,15 @@ ID_INLINE int idCollisionModelManagerLocal::TranslateEdgeThroughEdge( idVec3 &cr d = l2[4] * cross[0] + l2[5] * cross[1] + l2[2] * cross[2]; - if ( d == 0.0f ) { + if (d == 0.0f) { *fraction = 1.0f; // no collision ever return false; } - t = -l1.PermutedInnerProduct( l2 ); + t = -l1.PermutedInnerProduct(l2); // if the lines cross each other to begin with - if ( t == 0.0f ) { + if (t == 0.0f) { *fraction = 0.0f; return true; } @@ -181,9 +181,9 @@ ID_INLINE int idCollisionModelManagerLocal::TranslateEdgeThroughEdge( idVec3 &cr CM_AddContact ================ */ -ID_INLINE void CM_AddContact( cm_traceWork_t *tw ) { +ID_INLINE void CM_AddContact(cm_traceWork_t* tw) { - if ( tw->numContacts >= tw->maxContacts ) { + if (tw->numContacts >= tw->maxContacts) { return; } // copy contact information from trace_t @@ -200,11 +200,11 @@ CM_SetVertexSidedness stores for the given model vertex at which side of one of the trm edges it passes ================ */ -ID_INLINE void CM_SetVertexSidedness( cm_vertex_t *v, const idPluecker &vpl, const idPluecker &epl, const int bitNum ) { - if ( !(v->sideSet & (1<sideSet & (1 << bitNum))) { float fl; - fl = vpl.PermutedInnerProduct( epl ); - v->side = (v->side & ~(1<side = (v->side & ~(1 << bitNum)) | (FLOATSIGNBITSET(fl) << bitNum); v->sideSet |= (1 << bitNum); } } @@ -216,11 +216,11 @@ CM_SetEdgeSidedness stores for the given model edge at which side one of the trm vertices ================ */ -ID_INLINE void CM_SetEdgeSidedness( cm_edge_t *edge, const idPluecker &vpl, const idPluecker &epl, const int bitNum ) { - if ( !(edge->sideSet & (1<sideSet & (1 << bitNum))) { float fl; - fl = vpl.PermutedInnerProduct( epl ); - edge->side = (edge->side & ~(1<side = (edge->side & ~(1 << bitNum)) | (FLOATSIGNBITSET(fl) << bitNum); edge->sideSet |= (1 << bitNum); } } @@ -230,79 +230,79 @@ ID_INLINE void CM_SetEdgeSidedness( cm_edge_t *edge, const idPluecker &vpl, cons idCollisionModelManagerLocal::TranslateTrmEdgeThroughPolygon ================ */ -void idCollisionModelManagerLocal::TranslateTrmEdgeThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmEdge_t *trmEdge ) { +void idCollisionModelManagerLocal::TranslateTrmEdgeThroughPolygon(cm_traceWork_t* tw, cm_polygon_t* poly, cm_trmEdge_t* trmEdge) { int i, edgeNum; float f1, f2, dist, d1, d2; idVec3 start, end, normal; - cm_edge_t *edge; - cm_vertex_t *v1, *v2; - idPluecker *pl, epsPl; + cm_edge_t* edge; + cm_vertex_t* v1, * v2; + idPluecker* pl, epsPl; // check edges for a collision - for ( i = 0; i < poly->numEdges; i++) { + for (i = 0; i < poly->numEdges; i++) { edgeNum = poly->edges[i]; edge = tw->model->edges + abs(edgeNum); // if this edge is already checked - if ( edge->checkcount == idCollisionModelManagerLocal::checkCount ) { + if (edge->checkcount == idCollisionModelManagerLocal::checkCount) { continue; } // can never collide with internal edges - if ( edge->internal ) { + if (edge->internal) { continue; } pl = &tw->polygonEdgePlueckerCache[i]; // get the sides at which the trm edge vertices pass the polygon edge - CM_SetEdgeSidedness( edge, *pl, tw->vertices[trmEdge->vertexNum[0]].pl, trmEdge->vertexNum[0] ); - CM_SetEdgeSidedness( edge, *pl, tw->vertices[trmEdge->vertexNum[1]].pl, trmEdge->vertexNum[1] ); + CM_SetEdgeSidedness(edge, *pl, tw->vertices[trmEdge->vertexNum[0]].pl, trmEdge->vertexNum[0]); + CM_SetEdgeSidedness(edge, *pl, tw->vertices[trmEdge->vertexNum[1]].pl, trmEdge->vertexNum[1]); // if the trm edge start and end vertex do not pass the polygon edge at different sides - if ( !(((edge->side >> trmEdge->vertexNum[0]) ^ (edge->side >> trmEdge->vertexNum[1])) & 1) ) { + if (!(((edge->side >> trmEdge->vertexNum[0]) ^ (edge->side >> trmEdge->vertexNum[1])) & 1)) { continue; } // get the sides at which the polygon edge vertices pass the trm edge v1 = tw->model->vertices + edge->vertexNum[INTSIGNBITSET(edgeNum)]; - CM_SetVertexSidedness( v1, tw->polygonVertexPlueckerCache[i], trmEdge->pl, trmEdge->bitNum ); + CM_SetVertexSidedness(v1, tw->polygonVertexPlueckerCache[i], trmEdge->pl, trmEdge->bitNum); v2 = tw->model->vertices + edge->vertexNum[INTSIGNBITNOTSET(edgeNum)]; - CM_SetVertexSidedness( v2, tw->polygonVertexPlueckerCache[i+1], trmEdge->pl, trmEdge->bitNum ); + CM_SetVertexSidedness(v2, tw->polygonVertexPlueckerCache[i + 1], trmEdge->pl, trmEdge->bitNum); // if the polygon edge start and end vertex do not pass the trm edge at different sides - if ( !((v1->side ^ v2->side) & (1<bitNum)) ) { + if (!((v1->side ^ v2->side) & (1 << trmEdge->bitNum))) { continue; } // if there is no possible collision between the trm edge and the polygon edge - if ( !idCollisionModelManagerLocal::TranslateEdgeThroughEdge( trmEdge->cross, trmEdge->pl, *pl, &f1 ) ) { + if (!idCollisionModelManagerLocal::TranslateEdgeThroughEdge(trmEdge->cross, trmEdge->pl, *pl, &f1)) { continue; } // if moving away from edge - if ( f1 < 0.0f ) { + if (f1 < 0.0f) { continue; } // pluecker coordinate for epsilon expanded edge - epsPl.FromLine( tw->model->vertices[edge->vertexNum[0]].p + edge->normal * CM_CLIP_EPSILON, - tw->model->vertices[edge->vertexNum[1]].p + edge->normal * CM_CLIP_EPSILON ); + epsPl.FromLine(tw->model->vertices[edge->vertexNum[0]].p + edge->normal * CM_CLIP_EPSILON, + tw->model->vertices[edge->vertexNum[1]].p + edge->normal * CM_CLIP_EPSILON); // calculate collision fraction with epsilon expanded edge - if ( !idCollisionModelManagerLocal::TranslateEdgeThroughEdge( trmEdge->cross, trmEdge->pl, epsPl, &f2 ) ) { + if (!idCollisionModelManagerLocal::TranslateEdgeThroughEdge(trmEdge->cross, trmEdge->pl, epsPl, &f2)) { continue; } // if no collision with epsilon edge or moving away from edge - if ( f2 > 1.0f || f1 < f2 ) { + if (f2 > 1.0f || f1 < f2) { continue; } - if ( f2 < 0.0f ) { + if (f2 < 0.0f) { f2 = 0.0f; } - if ( f2 < tw->trace.fraction ) { + if (f2 < tw->trace.fraction) { tw->trace.fraction = f2; // create plane with normal vector orthogonal to both the polygon edge and the trm edge start = tw->model->vertices[edge->vertexNum[0]].p; end = tw->model->vertices[edge->vertexNum[1]].p; - tw->trace.c.normal = ( end - start ).Cross( trmEdge->end - trmEdge->start ); + tw->trace.c.normal = (end - start).Cross(trmEdge->end - trmEdge->start); // FIXME: do this normalize when we know the first collision tw->trace.c.normal.Normalize(); tw->trace.c.dist = tw->trace.c.normal * start; - // make sure the collision plane faces the trace model - if ( tw->trace.c.normal * trmEdge->start - tw->trace.c.dist < 0.0f ) { + // make sure the collision plane faces against the trace direction + if ((tw->dir * tw->trace.c.normal) > 0.0f) { tw->trace.c.normal = -tw->trace.c.normal; tw->trace.c.dist = -tw->trace.c.dist; } @@ -318,12 +318,15 @@ void idCollisionModelManagerLocal::TranslateTrmEdgeThroughPolygon( cm_traceWork_ dist = normal * trmEdge->start; d1 = normal * start - dist; d2 = normal * end - dist; - f1 = d1 / ( d1 - d2 ); + f1 = d1 / (d1 - d2); //assert( f1 >= 0.0f && f1 <= 1.0f ); - tw->trace.c.point = start + f1 * ( end - start ); + tw->trace.c.point = start + f1 * (end - start); // if retrieving contacts - if ( tw->getContacts ) { - CM_AddContact( tw ); + if (tw->getContacts) { + CM_AddContact(tw); + } + else if (tw->trace.fraction == 0.0f) { + return; } } } @@ -337,53 +340,53 @@ CM_TranslationPlaneFraction #if 0 -float CM_TranslationPlaneFraction( idPlane &plane, idVec3 &start, idVec3 &end ) { +float CM_TranslationPlaneFraction(idPlane& plane, idVec3& start, idVec3& end) { float d1, d2; - d2 = plane.Distance( end ); + d2 = plane.Distance(end); // if the end point is closer to the plane than an epsilon we still take it for a collision - if ( d2 >= CM_CLIP_EPSILON ) { + if (d2 >= CM_CLIP_EPSILON) { return 1.0f; } - d1 = plane.Distance( start ); + d1 = plane.Distance(start); // if completely behind the polygon - if ( d1 <= 0.0f ) { + if (d1 <= 0.0f) { return 1.0f; } // leaves polygon - if ( d1 <= d2 ) { + if (d1 <= d2) { return 1.0f; } - return (d1-CM_CLIP_EPSILON) / (d1-d2); + return (d1 - CM_CLIP_EPSILON) / (d1 - d2); } #else -float CM_TranslationPlaneFraction( idPlane &plane, idVec3 &start, idVec3 &end ) { +float CM_TranslationPlaneFraction(idPlane& plane, idVec3& start, idVec3& end) { float d1, d2, d2eps; - d2 = plane.Distance( end ); + d2 = plane.Distance(end); // if the end point is closer to the plane than an epsilon we still take it for a collision // if ( d2 >= CM_CLIP_EPSILON ) { d2eps = d2 - CM_CLIP_EPSILON; - if ( FLOATSIGNBITNOTSET(d2eps) ) { + if (FLOATSIGNBITNOTSET(d2eps)) { return 1.0f; } - d1 = plane.Distance( start ); + d1 = plane.Distance(start); // if completely behind the polygon - if ( FLOATSIGNBITSET(d1) ) { + if (FLOATSIGNBITSET(d1)) { return 1.0f; } // if going towards the front of the plane and // the start and end point are not at equal distance from the plane // if ( d1 > d2 ) d2 = d1 - d2; - if ( d2 <= 0.0f ) { + if (d2 <= 0.0f) { return 1.0f; } - return (d1-CM_CLIP_EPSILON) / d2; + return (d1 - CM_CLIP_EPSILON) / d2; } #endif @@ -393,23 +396,23 @@ float CM_TranslationPlaneFraction( idPlane &plane, idVec3 &start, idVec3 &end ) idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon ================ */ -void idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmVertex_t *v, int bitNum ) { +void idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon(cm_traceWork_t* tw, cm_polygon_t* poly, cm_trmVertex_t* v, int bitNum) { int i, edgeNum; float f; - cm_edge_t *edge; + cm_edge_t* edge; - f = CM_TranslationPlaneFraction( poly->plane, v->p, v->endp ); - if ( f < tw->trace.fraction ) { + f = CM_TranslationPlaneFraction(poly->plane, v->p, v->endp); + if (f < tw->trace.fraction) { - for ( i = 0; i < poly->numEdges; i++ ) { + for (i = 0; i < poly->numEdges; i++) { edgeNum = poly->edges[i]; edge = tw->model->edges + abs(edgeNum); - CM_SetEdgeSidedness( edge, tw->polygonEdgePlueckerCache[i], v->pl, bitNum ); - if ( INTSIGNBITSET(edgeNum) ^ ((edge->side >> bitNum) & 1) ) { + CM_SetEdgeSidedness(edge, tw->polygonEdgePlueckerCache[i], v->pl, bitNum); + if (INTSIGNBITSET(edgeNum) ^ ((edge->side >> bitNum) & 1)) { return; } } - if ( f < 0.0f ) { + if (f < 0.0f) { f = 0.0f; } tw->trace.fraction = f; @@ -419,12 +422,12 @@ void idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon( cm_traceWor tw->trace.c.contents = poly->contents; tw->trace.c.material = poly->material; tw->trace.c.type = CONTACT_TRMVERTEX; - tw->trace.c.modelFeature = *reinterpret_cast(&poly); + tw->trace.c.modelFeature = *reinterpret_cast(&poly); tw->trace.c.trmFeature = v - tw->vertices; - tw->trace.c.point = v->p + tw->trace.fraction * ( v->endp - v->p ); + tw->trace.c.point = v->p + tw->trace.fraction * (v->endp - v->p); // if retrieving contacts - if ( tw->getContacts ) { - CM_AddContact( tw ); + if (tw->getContacts) { + CM_AddContact(tw); // no need to store the trm vertex more than once as a contact v->used = false; } @@ -436,33 +439,33 @@ void idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon( cm_traceWor idCollisionModelManagerLocal::TranslatePointThroughPolygon ================ */ -void idCollisionModelManagerLocal::TranslatePointThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *poly, cm_trmVertex_t *v ) { +void idCollisionModelManagerLocal::TranslatePointThroughPolygon(cm_traceWork_t* tw, cm_polygon_t* poly, cm_trmVertex_t* v) { int i, edgeNum; float f; - cm_edge_t *edge; + cm_edge_t* edge; idPluecker pl; - f = CM_TranslationPlaneFraction( poly->plane, v->p, v->endp ); - if ( f < tw->trace.fraction ) { + f = CM_TranslationPlaneFraction(poly->plane, v->p, v->endp); + if (f < tw->trace.fraction) { - for ( i = 0; i < poly->numEdges; i++ ) { + for (i = 0; i < poly->numEdges; i++) { edgeNum = poly->edges[i]; edge = tw->model->edges + abs(edgeNum); // if we didn't yet calculate the sidedness for this edge - if ( edge->checkcount != idCollisionModelManagerLocal::checkCount ) { + if (edge->checkcount != idCollisionModelManagerLocal::checkCount) { float fl; edge->checkcount = idCollisionModelManagerLocal::checkCount; pl.FromLine(tw->model->vertices[edge->vertexNum[0]].p, tw->model->vertices[edge->vertexNum[1]].p); - fl = v->pl.PermutedInnerProduct( pl ); + fl = v->pl.PermutedInnerProduct(pl); edge->side = FLOATSIGNBITSET(fl); } // if the point passes the edge at the wrong side //if ( (edgeNum > 0) == edge->side ) { - if ( INTSIGNBITSET(edgeNum) ^ edge->side ) { + if (INTSIGNBITSET(edgeNum) ^ edge->side) { return; } } - if ( f < 0.0f ) { + if (f < 0.0f) { f = 0.0f; } tw->trace.fraction = f; @@ -472,12 +475,12 @@ void idCollisionModelManagerLocal::TranslatePointThroughPolygon( cm_traceWork_t tw->trace.c.contents = poly->contents; tw->trace.c.material = poly->material; tw->trace.c.type = CONTACT_TRMVERTEX; - tw->trace.c.modelFeature = *reinterpret_cast(&poly); + tw->trace.c.modelFeature = *reinterpret_cast(&poly); tw->trace.c.trmFeature = v - tw->vertices; - tw->trace.c.point = v->p + tw->trace.fraction * ( v->endp - v->p ); + tw->trace.c.point = v->p + tw->trace.fraction * (v->endp - v->p); // if retrieving contacts - if ( tw->getContacts ) { - CM_AddContact( tw ); + if (tw->getContacts) { + CM_AddContact(tw); // no need to store the trm vertex more than once as a contact v->used = false; } @@ -489,39 +492,39 @@ void idCollisionModelManagerLocal::TranslatePointThroughPolygon( cm_traceWork_t idCollisionModelManagerLocal::TranslateVertexThroughTrmPolygon ================ */ -void idCollisionModelManagerLocal::TranslateVertexThroughTrmPolygon( cm_traceWork_t *tw, cm_trmPolygon_t *trmpoly, cm_polygon_t *poly, cm_vertex_t *v, idVec3 &endp, idPluecker &pl ) { +void idCollisionModelManagerLocal::TranslateVertexThroughTrmPolygon(cm_traceWork_t* tw, cm_trmPolygon_t* trmpoly, cm_polygon_t* poly, cm_vertex_t* v, idVec3& endp, idPluecker& pl) { int i, edgeNum; float f; - cm_trmEdge_t *edge; + cm_trmEdge_t* edge; - f = CM_TranslationPlaneFraction( trmpoly->plane, v->p, endp ); - if ( f < tw->trace.fraction ) { + f = CM_TranslationPlaneFraction(trmpoly->plane, v->p, endp); + if (f < tw->trace.fraction) { - for ( i = 0; i < trmpoly->numEdges; i++ ) { + for (i = 0; i < trmpoly->numEdges; i++) { edgeNum = trmpoly->edges[i]; edge = tw->edges + abs(edgeNum); - CM_SetVertexSidedness( v, pl, edge->pl, edge->bitNum ); - if ( INTSIGNBITSET(edgeNum) ^ ((v->side >> edge->bitNum) & 1) ) { + CM_SetVertexSidedness(v, pl, edge->pl, edge->bitNum); + if (INTSIGNBITSET(edgeNum) ^ ((v->side >> edge->bitNum) & 1)) { return; } } - if ( f < 0.0f ) { + if (f < 0.0f) { f = 0.0f; } tw->trace.fraction = f; // collision plane is the inverse trm polygon plane tw->trace.c.normal = -trmpoly->plane.Normal(); - tw->trace.c.dist = -trmpoly->plane.Dist(); + tw->trace.c.dist = tw->trace.c.normal * v->p; tw->trace.c.contents = poly->contents; tw->trace.c.material = poly->material; tw->trace.c.type = CONTACT_MODELVERTEX; tw->trace.c.modelFeature = v - tw->model->vertices; tw->trace.c.trmFeature = trmpoly - tw->polys; - tw->trace.c.point = v->p + tw->trace.fraction * ( endp - v->p ); + tw->trace.c.point = v->p; // if retrieving contacts - if ( tw->getContacts ) { - CM_AddContact( tw ); + if (tw->getContacts) { + CM_AddContact(tw); } } } @@ -533,148 +536,148 @@ idCollisionModelManagerLocal::TranslateTrmThroughPolygon returns true if the polygon blocks the complete translation ================ */ -bool idCollisionModelManagerLocal::TranslateTrmThroughPolygon( cm_traceWork_t *tw, cm_polygon_t *p ) { +bool idCollisionModelManagerLocal::TranslateTrmThroughPolygon(cm_traceWork_t* tw, cm_polygon_t* p) { int i, j, k, edgeNum; float fraction, d; idVec3 endp; - idPluecker *pl; - cm_trmVertex_t *bv; - cm_trmEdge_t *be; - cm_trmPolygon_t *bp; - cm_vertex_t *v; - cm_edge_t *e; + idPluecker* pl; + cm_trmVertex_t* bv; + cm_trmEdge_t* be; + cm_trmPolygon_t* bp; + cm_vertex_t* v; + cm_edge_t* e; // if already checked this polygon - if ( p->checkcount == idCollisionModelManagerLocal::checkCount ) { + if (p->checkcount == idCollisionModelManagerLocal::checkCount) { return false; } p->checkcount = idCollisionModelManagerLocal::checkCount; // if this polygon does not have the right contents behind it - if ( !(p->contents & tw->contents) ) { + if (!(p->contents & tw->contents)) { return false; } // if the the trace bounds do not intersect the polygon bounds - if ( !tw->bounds.IntersectsBounds( p->bounds ) ) { + if (!tw->bounds.IntersectsBounds(p->bounds)) { return false; } // only collide with the polygon if approaching at the front - if ( ( p->plane.Normal() * tw->dir ) > 0.0f ) { + if ((p->plane.Normal() * tw->dir) > 0.0f) { return false; } // if the polygon is too far from the first heart plane - d = p->bounds.PlaneDistance( tw->heartPlane1 ); - if ( idMath::Fabs(d) > tw->maxDistFromHeartPlane1 ) { + d = p->bounds.PlaneDistance(tw->heartPlane1); + if (idMath::Fabs(d) > tw->maxDistFromHeartPlane1) { return false; } // if the polygon is too far from the second heart plane - d = p->bounds.PlaneDistance( tw->heartPlane2 ); - if ( idMath::Fabs(d) > tw->maxDistFromHeartPlane2 ) { + d = p->bounds.PlaneDistance(tw->heartPlane2); + if (idMath::Fabs(d) > tw->maxDistFromHeartPlane2) { return false; } fraction = tw->trace.fraction; // fast point trace - if ( tw->pointTrace ) { - idCollisionModelManagerLocal::TranslatePointThroughPolygon( tw, p, &tw->vertices[0] ); + if (tw->pointTrace) { + idCollisionModelManagerLocal::TranslatePointThroughPolygon(tw, p, &tw->vertices[0]); } else { // trace bounds should cross polygon plane - switch ( tw->bounds.PlaneSide( p->plane ) ) { - case PLANESIDE_CROSS: - break; - case PLANESIDE_FRONT: - if ( tw->model->isConvex ) { - tw->quickExit = true; - return true; - } - default: - return false; + switch (tw->bounds.PlaneSide(p->plane, 0.1f)) { + case PLANESIDE_CROSS: + break; + case PLANESIDE_FRONT: + if (tw->model->isConvex) { + tw->quickExit = true; + return true; + } + default: + return false; } // calculate pluecker coordinates for the polygon edges and polygon vertices - for ( i = 0; i < p->numEdges; i++ ) { + for (i = 0; i < p->numEdges; i++) { edgeNum = p->edges[i]; e = tw->model->edges + abs(edgeNum); // reset sidedness cache if this is the first time we encounter this edge during this trace - if ( e->checkcount != idCollisionModelManagerLocal::checkCount ) { + if (e->checkcount != idCollisionModelManagerLocal::checkCount) { e->sideSet = 0; } // pluecker coordinate for edge - tw->polygonEdgePlueckerCache[i].FromLine( tw->model->vertices[e->vertexNum[0]].p, - tw->model->vertices[e->vertexNum[1]].p ); + tw->polygonEdgePlueckerCache[i].FromLine(tw->model->vertices[e->vertexNum[0]].p, + tw->model->vertices[e->vertexNum[1]].p); v = &tw->model->vertices[e->vertexNum[INTSIGNBITSET(edgeNum)]]; // reset sidedness cache if this is the first time we encounter this vertex during this trace - if ( v->checkcount != idCollisionModelManagerLocal::checkCount ) { + if (v->checkcount != idCollisionModelManagerLocal::checkCount) { v->sideSet = 0; } // pluecker coordinate for vertex movement vector - tw->polygonVertexPlueckerCache[i].FromRay( v->p, -tw->dir ); + tw->polygonVertexPlueckerCache[i].FromRay(v->p, -tw->dir); } // copy first to last so we can easily cycle through for the edges tw->polygonVertexPlueckerCache[p->numEdges] = tw->polygonVertexPlueckerCache[0]; // trace trm vertices through polygon - for ( i = 0; i < tw->numVerts; i++ ) { + for (i = 0; i < tw->numVerts; i++) { bv = tw->vertices + i; - if ( bv->used ) { - idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon( tw, p, bv, i ); + if (bv->used) { + idCollisionModelManagerLocal::TranslateTrmVertexThroughPolygon(tw, p, bv, i); } } // trace trm edges through polygon - for ( i = 1; i <= tw->numEdges; i++ ) { + for (i = 1; i <= tw->numEdges; i++) { be = tw->edges + i; - if ( be->used ) { - idCollisionModelManagerLocal::TranslateTrmEdgeThroughPolygon( tw, p, be); + if (be->used) { + idCollisionModelManagerLocal::TranslateTrmEdgeThroughPolygon(tw, p, be); } } // trace all polygon vertices through the trm - for ( i = 0; i < p->numEdges; i++ ) { + for (i = 0; i < p->numEdges; i++) { edgeNum = p->edges[i]; e = tw->model->edges + abs(edgeNum); - if ( e->checkcount == idCollisionModelManagerLocal::checkCount ) { + if (e->checkcount == idCollisionModelManagerLocal::checkCount) { continue; } // set edge check count e->checkcount = idCollisionModelManagerLocal::checkCount; // can never collide with internal edges - if ( e->internal ) { + if (e->internal) { continue; } // got to check both vertices because we skip internal edges - for ( k = 0; k < 2; k++ ) { + for (k = 0; k < 2; k++) { v = tw->model->vertices + e->vertexNum[k ^ INTSIGNBITSET(edgeNum)]; // if this vertex is already checked - if ( v->checkcount == idCollisionModelManagerLocal::checkCount ) { + if (v->checkcount == idCollisionModelManagerLocal::checkCount) { continue; } // set vertex check count v->checkcount = idCollisionModelManagerLocal::checkCount; // if the vertex is outside the trace bounds - if ( !tw->bounds.ContainsPoint( v->p ) ) { + if (!tw->bounds.ContainsPoint(v->p)) { continue; } // vertex end point after movement endp = v->p - tw->dir; // pluecker coordinate for vertex movement vector - pl = &tw->polygonVertexPlueckerCache[i+k]; + pl = &tw->polygonVertexPlueckerCache[i + k]; - for ( j = 0; j < tw->numPolys; j++ ) { + for (j = 0; j < tw->numPolys; j++) { bp = tw->polys + j; - if ( bp->used ) { - idCollisionModelManagerLocal::TranslateVertexThroughTrmPolygon( tw, bp, p, v, endp, *pl ); + if (bp->used) { + idCollisionModelManagerLocal::TranslateVertexThroughTrmPolygon(tw, bp, p, v, endp, *pl); } } } @@ -682,12 +685,12 @@ bool idCollisionModelManagerLocal::TranslateTrmThroughPolygon( cm_traceWork_t *t } // if there was a collision with this polygon and we are not retrieving contacts - if ( tw->trace.fraction < fraction && !tw->getContacts ) { + if (tw->trace.fraction < fraction && !tw->getContacts) { fraction = tw->trace.fraction; endp = tw->start + fraction * tw->dir; // decrease bounds - for ( i = 0; i < 3; i++ ) { - if ( tw->start[i] < endp[i] ) { + for (i = 0; i < 3; i++) { + if (tw->start[i] < endp[i]) { tw->bounds[0][i] = tw->start[i] + tw->size[0][i] - CM_BOX_EPSILON; tw->bounds[1][i] = endp[i] + tw->size[1][i] + CM_BOX_EPSILON; } @@ -698,7 +701,7 @@ bool idCollisionModelManagerLocal::TranslateTrmThroughPolygon( cm_traceWork_t *t } } - return ( tw->trace.fraction == 0.0f ); + return (tw->trace.fraction == 0.0f); } /* @@ -706,30 +709,30 @@ bool idCollisionModelManagerLocal::TranslateTrmThroughPolygon( cm_traceWork_t *t idCollisionModelManagerLocal::SetupTrm ================ */ -void idCollisionModelManagerLocal::SetupTrm( cm_traceWork_t *tw, const idTraceModel *trm ) { +void idCollisionModelManagerLocal::SetupTrm(cm_traceWork_t* tw, const idTraceModel* trm) { int i, j; // vertices tw->numVerts = trm->numVerts; - for ( i = 0; i < trm->numVerts; i++ ) { + for (i = 0; i < trm->numVerts; i++) { tw->vertices[i].p = trm->verts[i]; tw->vertices[i].used = false; } // edges tw->numEdges = trm->numEdges; - for ( i = 1; i <= trm->numEdges; i++ ) { + for (i = 1; i <= trm->numEdges; i++) { tw->edges[i].vertexNum[0] = trm->edges[i].v[0]; tw->edges[i].vertexNum[1] = trm->edges[i].v[1]; tw->edges[i].used = false; } // polygons tw->numPolys = trm->numPolys; - for ( i = 0; i < trm->numPolys; i++ ) { + for (i = 0; i < trm->numPolys; i++) { tw->polys[i].numEdges = trm->polys[i].numEdges; - for ( j = 0; j < trm->polys[i].numEdges; j++ ) { + for (j = 0; j < trm->polys[i].numEdges; j++) { tw->polys[i].edges[j] = trm->polys[i].edges[j]; } - tw->polys[i].plane.SetNormal( trm->polys[i].normal ); + tw->polys[i].plane.SetNormal(trm->polys[i].normal); tw->polys[i].used = false; } // is the trace model convex or not @@ -741,17 +744,17 @@ void idCollisionModelManagerLocal::SetupTrm( cm_traceWork_t *tw, const idTraceMo idCollisionModelManagerLocal::SetupTranslationHeartPlanes ================ */ -void idCollisionModelManagerLocal::SetupTranslationHeartPlanes( cm_traceWork_t *tw ) { +void idCollisionModelManagerLocal::SetupTranslationHeartPlanes(cm_traceWork_t* tw) { idVec3 dir, normal1, normal2; // calculate trace heart planes dir = tw->dir; dir.Normalize(); - dir.NormalVectors( normal1, normal2 ); - tw->heartPlane1.SetNormal( normal1 ); - tw->heartPlane1.FitThroughPoint( tw->start ); - tw->heartPlane2.SetNormal( normal2 ); - tw->heartPlane2.FitThroughPoint( tw->start ); + dir.NormalVectors(normal1, normal2); + tw->heartPlane1.SetNormal(normal1); + tw->heartPlane1.FitThroughPoint(tw->start); + tw->heartPlane2.SetNormal(normal2); + tw->heartPlane2.FitThroughPoint(tw->start); } /* @@ -763,49 +766,55 @@ idCollisionModelManagerLocal::Translation static int entered = 0; #endif -void idCollisionModelManagerLocal::Translation( trace_t *results, const idVec3 &start, const idVec3 &end, - const idTraceModel *trm, const idMat3 &trmAxis, int contentMask, - cmHandle_t model, const idVec3 &modelOrigin, const idMat3 &modelAxis ) { +void idCollisionModelManagerLocal::Translation(trace_t* results, const idVec3& start, const idVec3& end, + const idTraceModel* trm, const idMat3& trmAxis, int contentMask, + cmHandle_t model, const idVec3& modelOrigin, const idMat3& modelAxis) { int i, j; float dist; bool model_rotated, trm_rotated; idVec3 dir1, dir2, dir; idMat3 invModelAxis, tmpAxis; - cm_trmPolygon_t *poly; - cm_trmEdge_t *edge; - cm_trmVertex_t *vert; - ALIGN16( static cm_traceWork_t tw ); + cm_trmPolygon_t* poly; + cm_trmEdge_t* edge; + cm_trmVertex_t* vert; + ALIGN16(static cm_traceWork_t tw); - assert( ((byte *)&start) < ((byte *)results) || ((byte *)&start) >= (((byte *)results) + sizeof( trace_t )) ); - assert( ((byte *)&end) < ((byte *)results) || ((byte *)&end) >= (((byte *)results) + sizeof( trace_t )) ); - assert( ((byte *)&trmAxis) < ((byte *)results) || ((byte *)&trmAxis) >= (((byte *)results) + sizeof( trace_t )) ); + assert(((byte*)&start) < ((byte*)results) || ((byte*)&start) >= (((byte*)results) + sizeof(trace_t))); + assert(((byte*)&end) < ((byte*)results) || ((byte*)&end) >= (((byte*)results) + sizeof(trace_t))); + assert(((byte*)&trmAxis) < ((byte*)results) || ((byte*)&trmAxis) >= (((byte*)results) + sizeof(trace_t))); - memset( results, 0, sizeof( *results ) ); + memset(results, 0, sizeof(*results)); - if ( model < 0 || model > MAX_SUBMODELS || model > idCollisionModelManagerLocal::maxModels ) { + if (model < 0 || model > MAX_SUBMODELS || model > idCollisionModelManagerLocal::maxModels) { common->Printf("idCollisionModelManagerLocal::Translation: invalid model handle\n"); + results->fraction = 1.0f; + results->endpos = end; + results->endAxis = trmAxis; return; } - if ( !idCollisionModelManagerLocal::models[model] ) { + if (!idCollisionModelManagerLocal::models[model]) { common->Printf("idCollisionModelManagerLocal::Translation: invalid model\n"); + results->fraction = 1.0f; + results->endpos = end; + results->endAxis = trmAxis; return; } // if case special position test - if ( start[0] == end[0] && start[1] == end[1] && start[2] == end[2] ) { - idCollisionModelManagerLocal::ContentsTrm( results, start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis ); + if (start[0] == end[0] && start[1] == end[1] && start[2] == end[2]) { + idCollisionModelManagerLocal::ContentsTrm(results, start, trm, trmAxis, contentMask, model, modelOrigin, modelAxis); return; } #ifdef _DEBUG bool startsolid = false; // test whether or not stuck to begin with - if ( cm_debugCollision.GetBool() ) { - if ( !entered && !idCollisionModelManagerLocal::getContacts ) { + if (cm_debugCollision.GetBool()) { + if (!entered && !idCollisionModelManagerLocal::getContacts) { entered = 1; // if already messed up to begin with - if ( idCollisionModelManagerLocal::Contents( start, trm, trmAxis, -1, model, modelOrigin, modelAxis ) & contentMask ) { + if (idCollisionModelManagerLocal::Contents(start, trm, trmAxis, -1, model, modelOrigin, modelAxis) & contentMask) { startsolid = true; } entered = 0; @@ -818,6 +827,7 @@ void idCollisionModelManagerLocal::Translation( trace_t *results, const idVec3 & tw.trace.fraction = 1.0f; tw.trace.c.contents = 0; tw.trace.c.type = CONTACT_NONE; + tw.trace.c.material = NULL; tw.contents = contentMask; tw.isConvex = true; tw.rotation = false; @@ -833,16 +843,16 @@ void idCollisionModelManagerLocal::Translation( trace_t *results, const idVec3 & tw.dir = end - start; model_rotated = modelAxis.IsRotated(); - if ( model_rotated ) { + if (model_rotated) { invModelAxis = modelAxis.Transpose(); } // if optimized point trace - if ( !trm || ( trm->bounds[1][0] - trm->bounds[0][0] <= 0.0f && - trm->bounds[1][1] - trm->bounds[0][1] <= 0.0f && - trm->bounds[1][2] - trm->bounds[0][2] <= 0.0f ) ) { + if (!trm || (trm->bounds[1][0] - trm->bounds[0][0] <= 0.0f && + trm->bounds[1][1] - trm->bounds[0][1] <= 0.0f && + trm->bounds[1][2] - trm->bounds[0][2] <= 0.0f)) { - if ( model_rotated ) { + if (model_rotated) { // rotate trace instead of model tw.start *= invModelAxis; tw.end *= invModelAxis; @@ -850,8 +860,8 @@ void idCollisionModelManagerLocal::Translation( trace_t *results, const idVec3 & } // trace bounds - for ( i = 0; i < 3; i++ ) { - if ( tw.start[i] < tw.end[i] ) { + for (i = 0; i < 3; i++) { + if (tw.start[i] < tw.end[i]) { tw.bounds[0][i] = tw.start[i] - CM_BOX_EPSILON; tw.bounds[1][i] = tw.end[i] + CM_BOX_EPSILON; } @@ -864,26 +874,26 @@ void idCollisionModelManagerLocal::Translation( trace_t *results, const idVec3 & tw.size.Zero(); // setup trace heart planes - idCollisionModelManagerLocal::SetupTranslationHeartPlanes( &tw ); + idCollisionModelManagerLocal::SetupTranslationHeartPlanes(&tw); tw.maxDistFromHeartPlane1 = CM_BOX_EPSILON; tw.maxDistFromHeartPlane2 = CM_BOX_EPSILON; // collision with single point tw.numVerts = 1; tw.vertices[0].p = tw.start; tw.vertices[0].endp = tw.vertices[0].p + tw.dir; - tw.vertices[0].pl.FromRay( tw.vertices[0].p, tw.dir ); + tw.vertices[0].pl.FromRay(tw.vertices[0].p, tw.dir); tw.numEdges = tw.numPolys = 0; tw.pointTrace = true; // trace through the model - idCollisionModelManagerLocal::TraceThroughModel( &tw ); + idCollisionModelManagerLocal::TraceThroughModel(&tw); // store results *results = tw.trace; results->endpos = start + results->fraction * (end - start); results->endAxis = mat3_identity; - if ( results->fraction < 1.0f ) { + if (results->fraction < 1.0f) { // rotate trace plane normal if there was a collision with a rotated model - if ( model_rotated ) { + if (model_rotated) { results->c.normal *= modelAxis; results->c.point *= modelAxis; } @@ -895,17 +905,17 @@ void idCollisionModelManagerLocal::Translation( trace_t *results, const idVec3 & } // the trace fraction is too inaccurate to describe translations over huge distances - if ( tw.dir.LengthSqr() > Square( CM_MAX_TRACE_DIST ) ) { + if (tw.dir.LengthSqr() > Square(CM_MAX_TRACE_DIST)) { results->fraction = 0.0f; results->endpos = start; results->endAxis = trmAxis; results->c.normal = vec3_origin; results->c.material = NULL; results->c.point = start; - if ( session->rw ) { - session->rw->DebugArrow( colorRed, start, end, 1 ); + if (session->rw) { + session->rw->DebugArrow(colorRed, start, end, 1); } - common->Printf( "idCollisionModelManagerLocal::Translation: huge translation\n" ); + common->Printf("idCollisionModelManagerLocal::Translation: huge translation\n"); return; } @@ -913,38 +923,39 @@ void idCollisionModelManagerLocal::Translation( trace_t *results, const idVec3 & tw.size.Clear(); // setup trm structure - idCollisionModelManagerLocal::SetupTrm( &tw, trm ); + idCollisionModelManagerLocal::SetupTrm(&tw, trm); trm_rotated = trmAxis.IsRotated(); // calculate vertex positions - if ( trm_rotated ) { - for ( i = 0; i < tw.numVerts; i++ ) { + if (trm_rotated) { + for (i = 0; i < tw.numVerts; i++) { // rotate trm around the start position tw.vertices[i].p *= trmAxis; } } - for ( i = 0; i < tw.numVerts; i++ ) { + for (i = 0; i < tw.numVerts; i++) { // set trm at start position tw.vertices[i].p += tw.start; } - if ( model_rotated ) { - for ( i = 0; i < tw.numVerts; i++ ) { + if (model_rotated) { + for (i = 0; i < tw.numVerts; i++) { // rotate trm around model instead of rotating the model tw.vertices[i].p *= invModelAxis; } } // add offset to start point - if ( trm_rotated ) { + if (trm_rotated) { dir = trm->offset * trmAxis; tw.start += dir; tw.end += dir; - } else { + } + else { tw.start += trm->offset; tw.end += trm->offset; } - if ( model_rotated ) { + if (model_rotated) { // rotate trace instead of model tw.start *= invModelAxis; tw.end *= invModelAxis; @@ -952,30 +963,32 @@ void idCollisionModelManagerLocal::Translation( trace_t *results, const idVec3 & } // rotate trm polygon planes - if ( trm_rotated & model_rotated ) { + if (trm_rotated & model_rotated) { tmpAxis = trmAxis * invModelAxis; - for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) { + for (poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++) { poly->plane *= tmpAxis; } - } else if ( trm_rotated ) { - for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) { + } + else if (trm_rotated) { + for (poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++) { poly->plane *= trmAxis; } - } else if ( model_rotated ) { - for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) { + } + else if (model_rotated) { + for (poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++) { poly->plane *= invModelAxis; } } // setup trm polygons - for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) { + for (poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++) { // if the trm poly plane is facing in the movement direction dist = poly->plane.Normal() * tw.dir; - if ( dist > 0.0f || ( !trm->isConvex && dist == 0.0f ) ) { + if (dist > 0.0f || (!trm->isConvex && dist == 0.0f)) { // this trm poly and it's edges and vertices need to be used for collision poly->used = true; - for ( j = 0; j < poly->numEdges; j++ ) { - edge = &tw.edges[abs( poly->edges[j] )]; + for (j = 0; j < poly->numEdges; j++) { + edge = &tw.edges[abs(poly->edges[j])]; edge->used = true; tw.vertices[edge->vertexNum[0]].used = true; tw.vertices[edge->vertexNum[1]].used = true; @@ -984,27 +997,27 @@ void idCollisionModelManagerLocal::Translation( trace_t *results, const idVec3 & } // setup trm vertices - for ( vert = tw.vertices, i = 0; i < tw.numVerts; i++, vert++ ) { - if ( !vert->used ) { + for (vert = tw.vertices, i = 0; i < tw.numVerts; i++, vert++) { + if (!vert->used) { continue; } // get axial trm size after rotations - tw.size.AddPoint( vert->p - tw.start ); + tw.size.AddPoint(vert->p - tw.start); // calculate the end position of each vertex for a full trace vert->endp = vert->p + tw.dir; // pluecker coordinate for vertex movement line - vert->pl.FromRay( vert->p, tw.dir ); + vert->pl.FromRay(vert->p, tw.dir); } // setup trm edges - for ( edge = tw.edges + 1, i = 1; i <= tw.numEdges; i++, edge++ ) { - if ( !edge->used ) { + for (edge = tw.edges + 1, i = 1; i <= tw.numEdges; i++, edge++) { + if (!edge->used) { continue; } // edge start, end and pluecker coordinate edge->start = tw.vertices[edge->vertexNum[0]].p; edge->end = tw.vertices[edge->vertexNum[1]].p; - edge->pl.FromLine( edge->start, edge->end ); + edge->pl.FromLine(edge->start, edge->end); // calculate normal of plane through movement plane created by the edge dir = edge->start - edge->end; edge->cross[0] = dir[0] * tw.dir[1] - dir[1] * tw.dir[0]; @@ -1015,43 +1028,45 @@ void idCollisionModelManagerLocal::Translation( trace_t *results, const idVec3 & } // set trm plane distances - for ( poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++ ) { - if ( poly->used ) { - poly->plane.FitThroughPoint( tw.edges[abs(poly->edges[0])].start ); + for (poly = tw.polys, i = 0; i < tw.numPolys; i++, poly++) { + if (poly->used) { + poly->plane.FitThroughPoint(tw.edges[abs(poly->edges[0])].start); } } // bounds for full trace, a little bit larger for epsilons - for ( i = 0; i < 3; i++ ) { - if ( tw.start[i] < tw.end[i] ) { + for (i = 0; i < 3; i++) { + if (tw.start[i] < tw.end[i]) { tw.bounds[0][i] = tw.start[i] + tw.size[0][i] - CM_BOX_EPSILON; tw.bounds[1][i] = tw.end[i] + tw.size[1][i] + CM_BOX_EPSILON; - } else { + } + else { tw.bounds[0][i] = tw.end[i] + tw.size[0][i] - CM_BOX_EPSILON; tw.bounds[1][i] = tw.start[i] + tw.size[1][i] + CM_BOX_EPSILON; } - if ( idMath::Fabs( tw.size[0][i] ) > idMath::Fabs( tw.size[1][i] ) ) { - tw.extents[i] = idMath::Fabs( tw.size[0][i] ) + CM_BOX_EPSILON; - } else { - tw.extents[i] = idMath::Fabs( tw.size[1][i] ) + CM_BOX_EPSILON; + if (idMath::Fabs(tw.size[0][i]) > idMath::Fabs(tw.size[1][i])) { + tw.extents[i] = idMath::Fabs(tw.size[0][i]) + CM_BOX_EPSILON; + } + else { + tw.extents[i] = idMath::Fabs(tw.size[1][i]) + CM_BOX_EPSILON; } } // setup trace heart planes - idCollisionModelManagerLocal::SetupTranslationHeartPlanes( &tw ); + idCollisionModelManagerLocal::SetupTranslationHeartPlanes(&tw); tw.maxDistFromHeartPlane1 = 0; tw.maxDistFromHeartPlane2 = 0; // calculate maximum trm vertex distance from both heart planes - for ( vert = tw.vertices, i = 0; i < tw.numVerts; i++, vert++ ) { - if ( !vert->used ) { + for (vert = tw.vertices, i = 0; i < tw.numVerts; i++, vert++) { + if (!vert->used) { continue; } - dist = idMath::Fabs( tw.heartPlane1.Distance( vert->p ) ); - if ( dist > tw.maxDistFromHeartPlane1 ) { + dist = idMath::Fabs(tw.heartPlane1.Distance(vert->p)); + if (dist > tw.maxDistFromHeartPlane1) { tw.maxDistFromHeartPlane1 = dist; } - dist = idMath::Fabs( tw.heartPlane2.Distance( vert->p ) ); - if ( dist > tw.maxDistFromHeartPlane2 ) { + dist = idMath::Fabs(tw.heartPlane2.Distance(vert->p)); + if (dist > tw.maxDistFromHeartPlane2) { tw.maxDistFromHeartPlane2 = dist; } } @@ -1060,37 +1075,38 @@ void idCollisionModelManagerLocal::Translation( trace_t *results, const idVec3 & tw.maxDistFromHeartPlane2 += CM_BOX_EPSILON; // trace through the model - idCollisionModelManagerLocal::TraceThroughModel( &tw ); + idCollisionModelManagerLocal::TraceThroughModel(&tw); // if we're getting contacts - if ( tw.getContacts ) { + if (tw.getContacts) { // move all contacts to world space - if ( model_rotated ) { - for ( i = 0; i < tw.numContacts; i++ ) { + if (model_rotated) { + for (i = 0; i < tw.numContacts; i++) { tw.contacts[i].normal *= modelAxis; tw.contacts[i].point *= modelAxis; } } - if ( modelOrigin != vec3_origin ) { - for ( i = 0; i < tw.numContacts; i++ ) { + if (modelOrigin != vec3_origin) { + for (i = 0; i < tw.numContacts; i++) { tw.contacts[i].point += modelOrigin; tw.contacts[i].dist += modelOrigin * tw.contacts[i].normal; } } idCollisionModelManagerLocal::numContacts = tw.numContacts; - } else { + } + else { // store results *results = tw.trace; - results->endpos = start + results->fraction * ( end - start ); + results->endpos = start + results->fraction * (end - start); results->endAxis = trmAxis; - if ( results->fraction < 1.0f ) { + if (results->fraction < 1.0f) { // if the fraction is tiny the actual movement could end up zero - if ( results->fraction > 0.0f && results->endpos.Compare( start ) ) { + if (results->fraction > 0.0f && results->endpos.Compare(start)) { results->fraction = 0.0f; } // rotate trace plane normal if there was a collision with a rotated model - if ( model_rotated ) { + if (model_rotated) { results->c.normal *= modelAxis; results->c.point *= modelAxis; } @@ -1101,17 +1117,17 @@ void idCollisionModelManagerLocal::Translation( trace_t *results, const idVec3 & #ifdef _DEBUG // test for missed collisions - if ( cm_debugCollision.GetBool() ) { - if ( !entered && !idCollisionModelManagerLocal::getContacts ) { + if (cm_debugCollision.GetBool()) { + if (!entered && !idCollisionModelManagerLocal::getContacts) { entered = 1; // if the trm is stuck in the model - if ( idCollisionModelManagerLocal::Contents( results->endpos, trm, trmAxis, -1, model, modelOrigin, modelAxis ) & contentMask ) { + if (idCollisionModelManagerLocal::Contents(results->endpos, trm, trmAxis, -1, model, modelOrigin, modelAxis) & contentMask) { trace_t tr; // test where the trm is stuck in the model - idCollisionModelManagerLocal::Contents( results->endpos, trm, trmAxis, -1, model, modelOrigin, modelAxis ); + idCollisionModelManagerLocal::Contents(results->endpos, trm, trmAxis, -1, model, modelOrigin, modelAxis); // re-run collision detection to find out where it failed - idCollisionModelManagerLocal::Translation( &tr, start, end, trm, trmAxis, contentMask, model, modelOrigin, modelAxis ); + idCollisionModelManagerLocal::Translation(&tr, start, end, trm, trmAxis, contentMask, model, modelOrigin, modelAxis); } entered = 0; } diff --git a/neo/game/physics/Physics_AF.cpp b/neo/game/physics/Physics_AF.cpp index 6f887a3e..67c6b88d 100644 --- a/neo/game/physics/Physics_AF.cpp +++ b/neo/game/physics/Physics_AF.cpp @@ -2,9 +2,9 @@ =========================================================================== Doom 3 GPL Source Code -Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. +Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. -This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?). +This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?). Doom 3 Source Code is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -31,30 +31,30 @@ If you have questions concerning this license or the applicable additional terms #include "../Game_local.h" -CLASS_DECLARATION( idPhysics_Base, idPhysics_AF ) +CLASS_DECLARATION(idPhysics_Base, idPhysics_AF) END_CLASS -const float ERROR_REDUCTION = 0.5f; -const float ERROR_REDUCTION_MAX = 256.0f; -const float LIMIT_ERROR_REDUCTION = 0.3f; -const float LCP_EPSILON = 1e-7f; -const float LIMIT_LCP_EPSILON = 1e-4f; -const float CONTACT_LCP_EPSILON = 1e-6f; -const float CENTER_OF_MASS_EPSILON = 1e-4f; -const float NO_MOVE_TIME = 1.0f; -const float NO_MOVE_TRANSLATION_TOLERANCE = 10.0f; -const float NO_MOVE_ROTATION_TOLERANCE = 10.0f; -const float MIN_MOVE_TIME = -1.0f; -const float MAX_MOVE_TIME = -1.0f; -const float IMPULSE_THRESHOLD = 500.0f; -const float SUSPEND_LINEAR_VELOCITY = 10.0f; -const float SUSPEND_ANGULAR_VELOCITY = 15.0f; -const float SUSPEND_LINEAR_ACCELERATION = 20.0f; -const float SUSPEND_ANGULAR_ACCELERATION = 30.0f; -const idVec6 vec6_lcp_epsilon = idVec6( LCP_EPSILON, LCP_EPSILON, LCP_EPSILON, - LCP_EPSILON, LCP_EPSILON, LCP_EPSILON ); +const float ERROR_REDUCTION = 0.5f; +const float ERROR_REDUCTION_MAX = 256.0f; +const float LIMIT_ERROR_REDUCTION = 0.3f; +const float LCP_EPSILON = 1e-7f; +const float LIMIT_LCP_EPSILON = 1e-4f; +const float CONTACT_LCP_EPSILON = 1e-6f; +const float CENTER_OF_MASS_EPSILON = 1e-4f; +const float NO_MOVE_TIME = 1.0f; +const float NO_MOVE_TRANSLATION_TOLERANCE = 10.0f; +const float NO_MOVE_ROTATION_TOLERANCE = 10.0f; +const float MIN_MOVE_TIME = -1.0f; +const float MAX_MOVE_TIME = -1.0f; +const float IMPULSE_THRESHOLD = 500.0f; +const float SUSPEND_LINEAR_VELOCITY = 10.0f; +const float SUSPEND_ANGULAR_VELOCITY = 15.0f; +const float SUSPEND_LINEAR_ACCELERATION = 20.0f; +const float SUSPEND_ANGULAR_ACCELERATION = 30.0f; +const idVec6 vec6_lcp_epsilon = idVec6(LCP_EPSILON, LCP_EPSILON, LCP_EPSILON, + LCP_EPSILON, LCP_EPSILON, LCP_EPSILON); -#define AF_TIMINGS +// #define AF_TIMINGS #ifdef AF_TIMINGS static int lastTimerReset = 0; @@ -75,31 +75,31 @@ static idTimer timer_total, timer_pc, timer_ac, timer_collision, timer_lcp; idAFConstraint::idAFConstraint ================ */ -idAFConstraint::idAFConstraint( void ) { - type = CONSTRAINT_INVALID; - name = "noname"; - body1 = NULL; - body2 = NULL; - physics = NULL; +idAFConstraint::idAFConstraint(void) { + type = CONSTRAINT_INVALID; + name = "noname"; + body1 = NULL; + body2 = NULL; + physics = NULL; - lo.Zero( 6 ); - lo.SubVec6(0) = -vec6_infinity; - hi.Zero( 6 ); - hi.SubVec6(0) = vec6_infinity; - e.SetSize( 6 ); - e.SubVec6(0) = vec6_lcp_epsilon; + lo.Zero(6); + lo.SubVec6(0) = -vec6_infinity; + hi.Zero(6); + hi.SubVec6(0) = vec6_infinity; + e.SetSize(6); + e.SubVec6(0) = vec6_lcp_epsilon; - boxConstraint = NULL; - boxIndex[0] = -1; - boxIndex[1] = -1; - boxIndex[2] = -1; - boxIndex[3] = -1; - boxIndex[4] = -1; - boxIndex[5] = -1; + boxConstraint = NULL; + boxIndex[0] = -1; + boxIndex[1] = -1; + boxIndex[2] = -1; + boxIndex[3] = -1; + boxIndex[4] = -1; + boxIndex[5] = -1; - firstIndex = 0; + firstIndex = 0; - memset( &fl, 0, sizeof( fl ) ); + memset(&fl, 0, sizeof(fl)); } /* @@ -107,7 +107,7 @@ idAFConstraint::idAFConstraint( void ) { idAFConstraint::~idAFConstraint ================ */ -idAFConstraint::~idAFConstraint( void ) { +idAFConstraint::~idAFConstraint(void) { } /* @@ -115,10 +115,10 @@ idAFConstraint::~idAFConstraint( void ) { idAFConstraint::SetBody1 ================ */ -void idAFConstraint::SetBody1( idAFBody *body ) { - if ( body1 != body) { +void idAFConstraint::SetBody1(idAFBody* body) { + if (body1 != body) { body1 = body; - if ( physics ) { + if (physics) { physics->SetChanged(); } } @@ -129,10 +129,10 @@ void idAFConstraint::SetBody1( idAFBody *body ) { idAFConstraint::SetBody2 ================ */ -void idAFConstraint::SetBody2( idAFBody *body ) { - if ( body2 != body ) { +void idAFConstraint::SetBody2(idAFBody* body) { + if (body2 != body) { body2 = body; - if ( physics ) { + if (physics) { physics->SetChanged(); } } @@ -143,7 +143,7 @@ void idAFConstraint::SetBody2( idAFBody *body ) { idAFConstraint::GetMultiplier ================ */ -const idVecX &idAFConstraint::GetMultiplier( void ) { +const idVecX& idAFConstraint::GetMultiplier(void) { return lm; } @@ -152,8 +152,8 @@ const idVecX &idAFConstraint::GetMultiplier( void ) { idAFConstraint::Evaluate ================ */ -void idAFConstraint::Evaluate( float invTimeStep ) { - assert( 0 ); +void idAFConstraint::Evaluate(float invTimeStep) { + assert(0); } /* @@ -161,7 +161,7 @@ void idAFConstraint::Evaluate( float invTimeStep ) { idAFConstraint::ApplyFriction ================ */ -void idAFConstraint::ApplyFriction( float invTimeStep ) { +void idAFConstraint::ApplyFriction(float invTimeStep) { } /* @@ -169,15 +169,15 @@ void idAFConstraint::ApplyFriction( float invTimeStep ) { idAFConstraint::GetForce ================ */ -void idAFConstraint::GetForce( idAFBody *body, idVec6 &force ) { +void idAFConstraint::GetForce(idAFBody* body, idVec6& force) { idVecX v; - v.SetData( 6, VECX_ALLOCA( 6 ) ); - if ( body == body1 ) { - J1.TransposeMultiply( v, lm ); + v.SetData(6, VECX_ALLOCA(6)); + if (body == body1) { + J1.TransposeMultiply(v, lm); } - else if ( body == body2 ) { - J2.TransposeMultiply( v, lm ); + else if (body == body2) { + J2.TransposeMultiply(v, lm); } else { v.Zero(); @@ -190,8 +190,8 @@ void idAFConstraint::GetForce( idAFBody *body, idVec6 &force ) { idAFConstraint::Translate ================ */ -void idAFConstraint::Translate( const idVec3 &translation ) { - assert( 0 ); +void idAFConstraint::Translate(const idVec3& translation) { + assert(0); } /* @@ -199,8 +199,8 @@ void idAFConstraint::Translate( const idVec3 &translation ) { idAFConstraint::Rotate ================ */ -void idAFConstraint::Rotate( const idRotation &rotation ) { - assert( 0 ); +void idAFConstraint::Rotate(const idRotation& rotation) { + assert(0); } /* @@ -208,7 +208,7 @@ void idAFConstraint::Rotate( const idRotation &rotation ) { idAFConstraint::GetCenter ================ */ -void idAFConstraint::GetCenter( idVec3 ¢er ) { +void idAFConstraint::GetCenter(idVec3& center) { center.Zero(); } @@ -217,7 +217,7 @@ void idAFConstraint::GetCenter( idVec3 ¢er ) { idAFConstraint::DebugDraw ================ */ -void idAFConstraint::DebugDraw( void ) { +void idAFConstraint::DebugDraw(void) { } /* @@ -225,13 +225,13 @@ void idAFConstraint::DebugDraw( void ) { idAFConstraint::InitSize ================ */ -void idAFConstraint::InitSize( int size ) { - J1.Zero( size, 6 ); - J2.Zero( size, 6 ); - c1.Zero( size ); - c2.Zero( size ); - s.Zero( size ); - lm.Zero( size ); +void idAFConstraint::InitSize(int size) { + J1.Zero(size, 6); + J2.Zero(size, 6); + c1.Zero(size); + c2.Zero(size); + s.Zero(size); + lm.Zero(size); } /* @@ -239,8 +239,8 @@ void idAFConstraint::InitSize( int size ) { idAFConstraint::Save ================ */ -void idAFConstraint::Save( idSaveGame *saveFile ) const { - saveFile->WriteInt( type ); +void idAFConstraint::Save(idSaveGame* saveFile) const { + saveFile->WriteInt(type); } /* @@ -248,10 +248,10 @@ void idAFConstraint::Save( idSaveGame *saveFile ) const { idAFConstraint::Restore ================ */ -void idAFConstraint::Restore( idRestoreGame *saveFile ) { +void idAFConstraint::Restore(idRestoreGame* saveFile) { constraintType_t t; - saveFile->ReadInt( (int &)t ); - assert( t == type ); + saveFile->ReadInt((int&)t); + assert(t == type); } @@ -266,13 +266,13 @@ void idAFConstraint::Restore( idRestoreGame *saveFile ) { idAFConstraint_Fixed::idAFConstraint_Fixed ================ */ -idAFConstraint_Fixed::idAFConstraint_Fixed( const idStr &name, idAFBody *body1, idAFBody *body2 ) { - assert( body1 ); +idAFConstraint_Fixed::idAFConstraint_Fixed(const idStr& name, idAFBody* body1, idAFBody* body2) { + assert(body1); type = CONSTRAINT_FIXED; this->name = name; this->body1 = body1; this->body2 = body2; - InitSize( 6 ); + InitSize(6); fl.allowPrimary = true; fl.noCollision = true; @@ -284,9 +284,9 @@ idAFConstraint_Fixed::idAFConstraint_Fixed( const idStr &name, idAFBody *body1, idAFConstraint_Fixed::InitOffset ================ */ -void idAFConstraint_Fixed::InitOffset( void ) { - if ( body2 ) { - offset = ( body1->GetWorldOrigin() - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose(); +void idAFConstraint_Fixed::InitOffset(void) { + if (body2) { + offset = (body1->GetWorldOrigin() - body2->GetWorldOrigin()) * body2->GetWorldAxis().Transpose(); relAxis = body1->GetWorldAxis() * body2->GetWorldAxis().Transpose(); } else { @@ -300,11 +300,11 @@ void idAFConstraint_Fixed::InitOffset( void ) { idAFConstraint_Fixed::SetBody1 ================ */ -void idAFConstraint_Fixed::SetBody1( idAFBody *body ) { - if ( body1 != body) { +void idAFConstraint_Fixed::SetBody1(idAFBody* body) { + if (body1 != body) { body1 = body; InitOffset(); - if ( physics ) { + if (physics) { physics->SetChanged(); } } @@ -315,11 +315,11 @@ void idAFConstraint_Fixed::SetBody1( idAFBody *body ) { idAFConstraint_Fixed::SetBody2 ================ */ -void idAFConstraint_Fixed::SetBody2( idAFBody *body ) { - if ( body2 != body ) { +void idAFConstraint_Fixed::SetBody2(idAFBody* body) { + if (body2 != body) { body2 = body; InitOffset(); - if ( physics ) { + if (physics) { physics->SetChanged(); } } @@ -330,15 +330,15 @@ void idAFConstraint_Fixed::SetBody2( idAFBody *body ) { idAFConstraint_Fixed::Evaluate ================ */ -void idAFConstraint_Fixed::Evaluate( float invTimeStep ) { +void idAFConstraint_Fixed::Evaluate(float invTimeStep) { idVec3 ofs, a2; idMat3 ax; idRotation r; - idAFBody *master; + idAFBody* master; master = body2 ? body2 : physics->GetMasterBody(); - if ( master ) { + if (master) { a2 = offset * master->GetWorldAxis(); ofs = a2 + master->GetWorldOrigin(); ax = relAxis * master->GetWorldAxis(); @@ -349,22 +349,22 @@ void idAFConstraint_Fixed::Evaluate( float invTimeStep ) { ax = relAxis; } - J1.Set( mat3_identity, mat3_zero, - mat3_zero, mat3_identity ); + J1.Set(mat3_identity, mat3_zero, + mat3_zero, mat3_identity); - if ( body2 ) { - J2.Set( -mat3_identity, SkewSymmetric( a2 ), - mat3_zero, -mat3_identity ); + if (body2) { + J2.Set(-mat3_identity, SkewSymmetric(a2), + mat3_zero, -mat3_identity); } else { - J2.Zero( 6, 6 ); + J2.Zero(6, 6); } - c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( ofs - body1->GetWorldOrigin() ); - r = ( body1->GetWorldAxis().Transpose() * ax ).ToRotation(); - c1.SubVec3(1) = -( invTimeStep * ERROR_REDUCTION ) * ( r.GetVec() * -(float) DEG2RAD( r.GetAngle() ) ); + c1.SubVec3(0) = -(invTimeStep * ERROR_REDUCTION) * (ofs - body1->GetWorldOrigin()); + r = (body1->GetWorldAxis().Transpose() * ax).ToRotation(); + c1.SubVec3(1) = -(invTimeStep * ERROR_REDUCTION) * (r.GetVec() * -(float)DEG2RAD(r.GetAngle())); - c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX ); + c1.Clamp(-ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX); } /* @@ -372,7 +372,7 @@ void idAFConstraint_Fixed::Evaluate( float invTimeStep ) { idAFConstraint_Fixed::ApplyFriction ================ */ -void idAFConstraint_Fixed::ApplyFriction( float invTimeStep ) { +void idAFConstraint_Fixed::ApplyFriction(float invTimeStep) { // no friction } @@ -381,8 +381,8 @@ void idAFConstraint_Fixed::ApplyFriction( float invTimeStep ) { idAFConstraint_Fixed::Translate ================ */ -void idAFConstraint_Fixed::Translate( const idVec3 &translation ) { - if ( !body2 ) { +void idAFConstraint_Fixed::Translate(const idVec3& translation) { + if (!body2) { offset += translation; } } @@ -392,8 +392,8 @@ void idAFConstraint_Fixed::Translate( const idVec3 &translation ) { idAFConstraint_Fixed::Rotate ================ */ -void idAFConstraint_Fixed::Rotate( const idRotation &rotation ) { - if ( !body2 ) { +void idAFConstraint_Fixed::Rotate(const idRotation& rotation) { + if (!body2) { offset *= rotation; relAxis *= rotation.ToMat3(); } @@ -404,7 +404,7 @@ void idAFConstraint_Fixed::Rotate( const idRotation &rotation ) { idAFConstraint_Fixed::GetCenter ================ */ -void idAFConstraint_Fixed::GetCenter( idVec3 ¢er ) { +void idAFConstraint_Fixed::GetCenter(idVec3& center) { center = body1->GetWorldOrigin(); } @@ -413,15 +413,15 @@ void idAFConstraint_Fixed::GetCenter( idVec3 ¢er ) { idAFConstraint_Fixed::DebugDraw ================ */ -void idAFConstraint_Fixed::DebugDraw( void ) { - idAFBody *master; +void idAFConstraint_Fixed::DebugDraw(void) { + idAFBody* master; master = body2 ? body2 : physics->GetMasterBody(); - if ( master ) { - gameRenderWorld->DebugLine( colorRed, body1->GetWorldOrigin(), master->GetWorldOrigin() ); + if (master) { + gameRenderWorld->DebugLine(colorRed, body1->GetWorldOrigin(), master->GetWorldOrigin()); } else { - gameRenderWorld->DebugLine( colorRed, body1->GetWorldOrigin(), vec3_origin ); + gameRenderWorld->DebugLine(colorRed, body1->GetWorldOrigin(), vec3_origin); } } @@ -430,10 +430,10 @@ void idAFConstraint_Fixed::DebugDraw( void ) { idAFConstraint_Fixed::Save ================ */ -void idAFConstraint_Fixed::Save( idSaveGame *saveFile ) const { - idAFConstraint::Save( saveFile ); - saveFile->WriteVec3( offset ); - saveFile->WriteMat3( relAxis ); +void idAFConstraint_Fixed::Save(idSaveGame* saveFile) const { + idAFConstraint::Save(saveFile); + saveFile->WriteVec3(offset); + saveFile->WriteMat3(relAxis); } /* @@ -441,10 +441,10 @@ void idAFConstraint_Fixed::Save( idSaveGame *saveFile ) const { idAFConstraint_Fixed::Restore ================ */ -void idAFConstraint_Fixed::Restore( idRestoreGame *saveFile ) { - idAFConstraint::Restore( saveFile ); - saveFile->ReadVec3( offset ); - saveFile->ReadMat3( relAxis ); +void idAFConstraint_Fixed::Restore(idRestoreGame* saveFile) { + idAFConstraint::Restore(saveFile); + saveFile->ReadVec3(offset); + saveFile->ReadMat3(relAxis); } @@ -459,13 +459,13 @@ void idAFConstraint_Fixed::Restore( idRestoreGame *saveFile ) { idAFConstraint_BallAndSocketJoint::idAFConstraint_BallAndSocketJoint ================ */ -idAFConstraint_BallAndSocketJoint::idAFConstraint_BallAndSocketJoint( const idStr &name, idAFBody *body1, idAFBody *body2 ) { - assert( body1 ); +idAFConstraint_BallAndSocketJoint::idAFConstraint_BallAndSocketJoint(const idStr& name, idAFBody* body1, idAFBody* body2) { + assert(body1); type = CONSTRAINT_BALLANDSOCKETJOINT; this->name = name; this->body1 = body1; this->body2 = body2; - InitSize( 3 ); + InitSize(3); coneLimit = NULL; pyramidLimit = NULL; friction = 0.0f; @@ -479,11 +479,11 @@ idAFConstraint_BallAndSocketJoint::idAFConstraint_BallAndSocketJoint( const idSt idAFConstraint_BallAndSocketJoint::~idAFConstraint_BallAndSocketJoint ================ */ -idAFConstraint_BallAndSocketJoint::~idAFConstraint_BallAndSocketJoint( void ) { - if ( coneLimit ) { +idAFConstraint_BallAndSocketJoint::~idAFConstraint_BallAndSocketJoint(void) { + if (coneLimit) { delete coneLimit; } - if ( pyramidLimit ) { + if (pyramidLimit) { delete pyramidLimit; } } @@ -493,23 +493,23 @@ idAFConstraint_BallAndSocketJoint::~idAFConstraint_BallAndSocketJoint( void ) { idAFConstraint_BallAndSocketJoint::SetAnchor ================ */ -void idAFConstraint_BallAndSocketJoint::SetAnchor( const idVec3 &worldPosition ) { +void idAFConstraint_BallAndSocketJoint::SetAnchor(const idVec3& worldPosition) { // get anchor relative to center of mass of body1 - anchor1 = ( worldPosition - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose(); - if ( body2 ) { + anchor1 = (worldPosition - body1->GetWorldOrigin()) * body1->GetWorldAxis().Transpose(); + if (body2) { // get anchor relative to center of mass of body2 - anchor2 = ( worldPosition - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose(); + anchor2 = (worldPosition - body2->GetWorldOrigin()) * body2->GetWorldAxis().Transpose(); } else { anchor2 = worldPosition; } - if ( coneLimit ) { - coneLimit->SetAnchor( anchor2 ); + if (coneLimit) { + coneLimit->SetAnchor(anchor2); } - if ( pyramidLimit ) { - pyramidLimit->SetAnchor( anchor2 ); + if (pyramidLimit) { + pyramidLimit->SetAnchor(anchor2); } } @@ -518,8 +518,8 @@ void idAFConstraint_BallAndSocketJoint::SetAnchor( const idVec3 &worldPosition ) idAFConstraint_BallAndSocketJoint::GetAnchor ================ */ -idVec3 idAFConstraint_BallAndSocketJoint::GetAnchor( void ) const { - if ( body2 ) { +idVec3 idAFConstraint_BallAndSocketJoint::GetAnchor(void) const { + if (body2) { return body2->GetWorldOrigin() + body2->GetWorldAxis() * anchor2; } return anchor2; @@ -530,12 +530,12 @@ idVec3 idAFConstraint_BallAndSocketJoint::GetAnchor( void ) const { idAFConstraint_BallAndSocketJoint::SetNoLimit ================ */ -void idAFConstraint_BallAndSocketJoint::SetNoLimit( void ) { - if ( coneLimit ) { +void idAFConstraint_BallAndSocketJoint::SetNoLimit(void) { + if (coneLimit) { delete coneLimit; coneLimit = NULL; } - if ( pyramidLimit ) { + if (pyramidLimit) { delete pyramidLimit; pyramidLimit = NULL; } @@ -546,20 +546,20 @@ void idAFConstraint_BallAndSocketJoint::SetNoLimit( void ) { idAFConstraint_BallAndSocketJoint::SetConeLimit ================ */ -void idAFConstraint_BallAndSocketJoint::SetConeLimit( const idVec3 &coneAxis, const float coneAngle, const idVec3 &body1Axis ) { - if ( pyramidLimit ) { +void idAFConstraint_BallAndSocketJoint::SetConeLimit(const idVec3& coneAxis, const float coneAngle, const idVec3& body1Axis) { + if (pyramidLimit) { delete pyramidLimit; pyramidLimit = NULL; } - if ( !coneLimit ) { + if (!coneLimit) { coneLimit = new idAFConstraint_ConeLimit; - coneLimit->SetPhysics( physics ); + coneLimit->SetPhysics(physics); } - if ( body2 ) { - coneLimit->Setup( body1, body2, anchor2, coneAxis * body2->GetWorldAxis().Transpose(), coneAngle, body1Axis * body1->GetWorldAxis().Transpose() ); + if (body2) { + coneLimit->Setup(body1, body2, anchor2, coneAxis * body2->GetWorldAxis().Transpose(), coneAngle, body1Axis * body1->GetWorldAxis().Transpose()); } else { - coneLimit->Setup( body1, body2, anchor2, coneAxis, coneAngle, body1Axis * body1->GetWorldAxis().Transpose() ); + coneLimit->Setup(body1, body2, anchor2, coneAxis, coneAngle, body1Axis * body1->GetWorldAxis().Transpose()); } } @@ -568,24 +568,24 @@ void idAFConstraint_BallAndSocketJoint::SetConeLimit( const idVec3 &coneAxis, co idAFConstraint_BallAndSocketJoint::SetPyramidLimit ================ */ -void idAFConstraint_BallAndSocketJoint::SetPyramidLimit( const idVec3 &pyramidAxis, const idVec3 &baseAxis, - const float angle1, const float angle2, const idVec3 &body1Axis ) { - if ( coneLimit ) { +void idAFConstraint_BallAndSocketJoint::SetPyramidLimit(const idVec3& pyramidAxis, const idVec3& baseAxis, + const float angle1, const float angle2, const idVec3& body1Axis) { + if (coneLimit) { delete coneLimit; coneLimit = NULL; } - if ( !pyramidLimit ) { + if (!pyramidLimit) { pyramidLimit = new idAFConstraint_PyramidLimit; - pyramidLimit->SetPhysics( physics ); + pyramidLimit->SetPhysics(physics); } - if ( body2 ) { - pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis * body2->GetWorldAxis().Transpose(), - baseAxis * body2->GetWorldAxis().Transpose(), angle1, angle2, - body1Axis * body1->GetWorldAxis().Transpose() ); + if (body2) { + pyramidLimit->Setup(body1, body2, anchor2, pyramidAxis * body2->GetWorldAxis().Transpose(), + baseAxis * body2->GetWorldAxis().Transpose(), angle1, angle2, + body1Axis * body1->GetWorldAxis().Transpose()); } else { - pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis, baseAxis, angle1, angle2, - body1Axis * body1->GetWorldAxis().Transpose() ); + pyramidLimit->Setup(body1, body2, anchor2, pyramidAxis, baseAxis, angle1, angle2, + body1Axis * body1->GetWorldAxis().Transpose()); } } @@ -594,12 +594,12 @@ void idAFConstraint_BallAndSocketJoint::SetPyramidLimit( const idVec3 &pyramidAx idAFConstraint_BallAndSocketJoint::SetLimitEpsilon ================ */ -void idAFConstraint_BallAndSocketJoint::SetLimitEpsilon( const float e ) { - if ( coneLimit ) { - coneLimit->SetEpsilon( e ); +void idAFConstraint_BallAndSocketJoint::SetLimitEpsilon(const float e) { + if (coneLimit) { + coneLimit->SetEpsilon(e); } - if ( pyramidLimit ) { - pyramidLimit->SetEpsilon( e ); + if (pyramidLimit) { + pyramidLimit->SetEpsilon(e); } } @@ -608,8 +608,8 @@ void idAFConstraint_BallAndSocketJoint::SetLimitEpsilon( const float e ) { idAFConstraint_BallAndSocketJoint::GetFriction ================ */ -float idAFConstraint_BallAndSocketJoint::GetFriction( void ) const { - if ( af_forceFriction.GetFloat() > 0.0f ) { +float idAFConstraint_BallAndSocketJoint::GetFriction(void) const { + if (af_forceFriction.GetFloat() > 0.0f) { return af_forceFriction.GetFloat(); } return friction * physics->GetJointFrictionScale(); @@ -620,38 +620,38 @@ float idAFConstraint_BallAndSocketJoint::GetFriction( void ) const { idAFConstraint_BallAndSocketJoint::Evaluate ================ */ -void idAFConstraint_BallAndSocketJoint::Evaluate( float invTimeStep ) { +void idAFConstraint_BallAndSocketJoint::Evaluate(float invTimeStep) { idVec3 a1, a2; - idAFBody *master; + idAFBody* master; master = body2 ? body2 : physics->GetMasterBody(); a1 = anchor1 * body1->GetWorldAxis(); - if ( master ) { + if (master) { a2 = anchor2 * master->GetWorldAxis(); - c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 + master->GetWorldOrigin() - ( a1 + body1->GetWorldOrigin() ) ); + c1.SubVec3(0) = -(invTimeStep * ERROR_REDUCTION) * (a2 + master->GetWorldOrigin() - (a1 + body1->GetWorldOrigin())); } else { - c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( anchor2 - ( a1 + body1->GetWorldOrigin() ) ); + c1.SubVec3(0) = -(invTimeStep * ERROR_REDUCTION) * (anchor2 - (a1 + body1->GetWorldOrigin())); } - c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX ); + c1.Clamp(-ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX); - J1.Set( mat3_identity, -SkewSymmetric( a1 ) ); + J1.Set(mat3_identity, -SkewSymmetric(a1)); - if ( body2 ) { - J2.Set( -mat3_identity, SkewSymmetric( a2 ) ); + if (body2) { + J2.Set(-mat3_identity, SkewSymmetric(a2)); } else { - J2.Zero( 3, 6 ); + J2.Zero(3, 6); } - if ( coneLimit ) { - coneLimit->Add( physics, invTimeStep ); + if (coneLimit) { + coneLimit->Add(physics, invTimeStep); } - else if ( pyramidLimit ) { - pyramidLimit->Add( physics, invTimeStep ); + else if (pyramidLimit) { + pyramidLimit->Add(physics, invTimeStep); } } @@ -660,39 +660,39 @@ void idAFConstraint_BallAndSocketJoint::Evaluate( float invTimeStep ) { idAFConstraint_BallAndSocketJoint::ApplyFriction ================ */ -void idAFConstraint_BallAndSocketJoint::ApplyFriction( float invTimeStep ) { +void idAFConstraint_BallAndSocketJoint::ApplyFriction(float invTimeStep) { idVec3 angular; float invMass, currentFriction; currentFriction = GetFriction(); - if ( currentFriction <= 0.0f ) { + if (currentFriction <= 0.0f) { return; } - if ( af_useImpulseFriction.GetBool() || af_useJointImpulseFriction.GetBool() ) { + if (af_useImpulseFriction.GetBool() || af_useJointImpulseFriction.GetBool()) { angular = body1->GetAngularVelocity(); invMass = body1->GetInverseMass(); - if ( body2 ) { + if (body2) { angular -= body2->GetAngularVelocity(); invMass += body2->GetInverseMass(); } angular *= currentFriction / invMass; - body1->SetAngularVelocity( body1->GetAngularVelocity() - angular * body1->GetInverseMass() ); - if ( body2 ) { - body2->SetAngularVelocity( body2->GetAngularVelocity() + angular * body2->GetInverseMass() ); + body1->SetAngularVelocity(body1->GetAngularVelocity() - angular * body1->GetInverseMass()); + if (body2) { + body2->SetAngularVelocity(body2->GetAngularVelocity() + angular * body2->GetInverseMass()); } } else { - if ( !fc ) { + if (!fc) { fc = new idAFConstraint_BallAndSocketJointFriction; - fc->Setup( this ); + fc->Setup(this); } - fc->Add( physics, invTimeStep ); + fc->Add(physics, invTimeStep); } } @@ -701,8 +701,8 @@ void idAFConstraint_BallAndSocketJoint::ApplyFriction( float invTimeStep ) { idAFConstraint_BallAndSocketJoint::GetForce ================ */ -void idAFConstraint_BallAndSocketJoint::GetForce( idAFBody *body, idVec6 &force ) { - idAFConstraint::GetForce( body, force ); +void idAFConstraint_BallAndSocketJoint::GetForce(idAFBody* body, idVec6& force) { + idAFConstraint::GetForce(body, force); // FIXME: add limit force } @@ -711,15 +711,15 @@ void idAFConstraint_BallAndSocketJoint::GetForce( idAFBody *body, idVec6 &force idAFConstraint_BallAndSocketJoint::Translate ================ */ -void idAFConstraint_BallAndSocketJoint::Translate( const idVec3 &translation ) { - if ( !body2 ) { +void idAFConstraint_BallAndSocketJoint::Translate(const idVec3& translation) { + if (!body2) { anchor2 += translation; } - if ( coneLimit ) { - coneLimit->Translate( translation ); + if (coneLimit) { + coneLimit->Translate(translation); } - else if ( pyramidLimit ) { - pyramidLimit->Translate( translation ); + else if (pyramidLimit) { + pyramidLimit->Translate(translation); } } @@ -728,15 +728,15 @@ void idAFConstraint_BallAndSocketJoint::Translate( const idVec3 &translation ) { idAFConstraint_BallAndSocketJoint::Rotate ================ */ -void idAFConstraint_BallAndSocketJoint::Rotate( const idRotation &rotation ) { - if ( !body2 ) { +void idAFConstraint_BallAndSocketJoint::Rotate(const idRotation& rotation) { + if (!body2) { anchor2 *= rotation; } - if ( coneLimit ) { - coneLimit->Rotate( rotation ); + if (coneLimit) { + coneLimit->Rotate(rotation); } - else if ( pyramidLimit ) { - pyramidLimit->Rotate( rotation ); + else if (pyramidLimit) { + pyramidLimit->Rotate(rotation); } } @@ -745,7 +745,7 @@ void idAFConstraint_BallAndSocketJoint::Rotate( const idRotation &rotation ) { idAFConstraint_BallAndSocketJoint::GetCenter ================ */ -void idAFConstraint_BallAndSocketJoint::GetCenter( idVec3 ¢er ) { +void idAFConstraint_BallAndSocketJoint::GetCenter(idVec3& center) { center = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); } @@ -754,17 +754,17 @@ void idAFConstraint_BallAndSocketJoint::GetCenter( idVec3 ¢er ) { idAFConstraint_BallAndSocketJoint::DebugDraw ================ */ -void idAFConstraint_BallAndSocketJoint::DebugDraw( void ) { +void idAFConstraint_BallAndSocketJoint::DebugDraw(void) { idVec3 a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); - gameRenderWorld->DebugLine( colorBlue, a1 - idVec3( 5, 0, 0 ), a1 + idVec3( 5, 0, 0 ) ); - gameRenderWorld->DebugLine( colorBlue, a1 - idVec3( 0, 5, 0 ), a1 + idVec3( 0, 5, 0 ) ); - gameRenderWorld->DebugLine( colorBlue, a1 - idVec3( 0, 0, 5 ), a1 + idVec3( 0, 0, 5 ) ); + gameRenderWorld->DebugLine(colorBlue, a1 - idVec3(5, 0, 0), a1 + idVec3(5, 0, 0)); + gameRenderWorld->DebugLine(colorBlue, a1 - idVec3(0, 5, 0), a1 + idVec3(0, 5, 0)); + gameRenderWorld->DebugLine(colorBlue, a1 - idVec3(0, 0, 5), a1 + idVec3(0, 0, 5)); - if ( af_showLimits.GetBool() ) { - if ( coneLimit ) { + if (af_showLimits.GetBool()) { + if (coneLimit) { coneLimit->DebugDraw(); } - if ( pyramidLimit ) { + if (pyramidLimit) { pyramidLimit->DebugDraw(); } } @@ -775,16 +775,16 @@ void idAFConstraint_BallAndSocketJoint::DebugDraw( void ) { idAFConstraint_BallAndSocketJoint::Save ================ */ -void idAFConstraint_BallAndSocketJoint::Save( idSaveGame *saveFile ) const { - idAFConstraint::Save( saveFile ); - saveFile->WriteVec3( anchor1 ); - saveFile->WriteVec3( anchor2 ); - saveFile->WriteFloat( friction ); - if ( coneLimit ) { - coneLimit->Save( saveFile ); +void idAFConstraint_BallAndSocketJoint::Save(idSaveGame* saveFile) const { + idAFConstraint::Save(saveFile); + saveFile->WriteVec3(anchor1); + saveFile->WriteVec3(anchor2); + saveFile->WriteFloat(friction); + if (coneLimit) { + coneLimit->Save(saveFile); } - if ( pyramidLimit ) { - pyramidLimit->Save( saveFile ); + if (pyramidLimit) { + pyramidLimit->Save(saveFile); } } @@ -793,16 +793,16 @@ void idAFConstraint_BallAndSocketJoint::Save( idSaveGame *saveFile ) const { idAFConstraint_BallAndSocketJoint::Restore ================ */ -void idAFConstraint_BallAndSocketJoint::Restore( idRestoreGame *saveFile ) { - idAFConstraint::Restore( saveFile ); - saveFile->ReadVec3( anchor1 ); - saveFile->ReadVec3( anchor2 ); - saveFile->ReadFloat( friction ); - if ( coneLimit ) { - coneLimit->Restore( saveFile ); +void idAFConstraint_BallAndSocketJoint::Restore(idRestoreGame* saveFile) { + idAFConstraint::Restore(saveFile); + saveFile->ReadVec3(anchor1); + saveFile->ReadVec3(anchor2); + saveFile->ReadFloat(friction); + if (coneLimit) { + coneLimit->Restore(saveFile); } - if ( pyramidLimit ) { - pyramidLimit->Restore( saveFile ); + if (pyramidLimit) { + pyramidLimit->Restore(saveFile); } } @@ -818,10 +818,10 @@ void idAFConstraint_BallAndSocketJoint::Restore( idRestoreGame *saveFile ) { idAFConstraint_BallAndSocketJointFriction::idAFConstraint_BallAndSocketJointFriction ================ */ -idAFConstraint_BallAndSocketJointFriction::idAFConstraint_BallAndSocketJointFriction( void ) { +idAFConstraint_BallAndSocketJointFriction::idAFConstraint_BallAndSocketJointFriction(void) { type = CONSTRAINT_FRICTION; name = "ballAndSocketJointFriction"; - InitSize( 3 ); + InitSize(3); joint = NULL; fl.allowPrimary = false; fl.frameConstraint = true; @@ -832,7 +832,7 @@ idAFConstraint_BallAndSocketJointFriction::idAFConstraint_BallAndSocketJointFric idAFConstraint_BallAndSocketJointFriction::Setup ================ */ -void idAFConstraint_BallAndSocketJointFriction::Setup( idAFConstraint_BallAndSocketJoint *bsj ) { +void idAFConstraint_BallAndSocketJointFriction::Setup(idAFConstraint_BallAndSocketJoint* bsj) { this->joint = bsj; body1 = bsj->GetBody1(); body2 = bsj->GetBody2(); @@ -843,7 +843,7 @@ void idAFConstraint_BallAndSocketJointFriction::Setup( idAFConstraint_BallAndSoc idAFConstraint_BallAndSocketJointFriction::Evaluate ================ */ -void idAFConstraint_BallAndSocketJointFriction::Evaluate( float invTimeStep ) { +void idAFConstraint_BallAndSocketJointFriction::Evaluate(float invTimeStep) { // do nothing } @@ -852,7 +852,7 @@ void idAFConstraint_BallAndSocketJointFriction::Evaluate( float invTimeStep ) { idAFConstraint_BallAndSocketJointFriction::ApplyFriction ================ */ -void idAFConstraint_BallAndSocketJointFriction::ApplyFriction( float invTimeStep ) { +void idAFConstraint_BallAndSocketJointFriction::ApplyFriction(float invTimeStep) { // do nothing } @@ -861,29 +861,29 @@ void idAFConstraint_BallAndSocketJointFriction::ApplyFriction( float invTimeStep idAFConstraint_BallAndSocketJointFriction::Add ================ */ -bool idAFConstraint_BallAndSocketJointFriction::Add( idPhysics_AF *phys, float invTimeStep ) { +bool idAFConstraint_BallAndSocketJointFriction::Add(idPhysics_AF* phys, float invTimeStep) { float f; physics = phys; f = joint->GetFriction() * joint->GetMultiplier().Length(); - if ( f == 0.0f ) { + if (f == 0.0f) { return false; } lo[0] = lo[1] = lo[2] = -f; hi[0] = hi[1] = hi[2] = f; - J1.Zero( 3, 6 ); + J1.Zero(3, 6); J1[0][3] = J1[1][4] = J1[2][5] = 1.0f; - if ( body2 ) { + if (body2) { - J2.Zero( 3, 6 ); + J2.Zero(3, 6); J2[0][3] = J2[1][4] = J2[2][5] = 1.0f; } - physics->AddFrameConstraint( this ); + physics->AddFrameConstraint(this); return true; } @@ -893,7 +893,7 @@ bool idAFConstraint_BallAndSocketJointFriction::Add( idPhysics_AF *phys, float i idAFConstraint_BallAndSocketJointFriction::Translate ================ */ -void idAFConstraint_BallAndSocketJointFriction::Translate( const idVec3 &translation ) { +void idAFConstraint_BallAndSocketJointFriction::Translate(const idVec3& translation) { } /* @@ -901,7 +901,7 @@ void idAFConstraint_BallAndSocketJointFriction::Translate( const idVec3 &transla idAFConstraint_BallAndSocketJointFriction::Rotate ================ */ -void idAFConstraint_BallAndSocketJointFriction::Rotate( const idRotation &rotation ) { +void idAFConstraint_BallAndSocketJointFriction::Rotate(const idRotation& rotation) { } @@ -916,13 +916,13 @@ void idAFConstraint_BallAndSocketJointFriction::Rotate( const idRotation &rotati idAFConstraint_UniversalJoint::idAFConstraint_UniversalJoint ================ */ -idAFConstraint_UniversalJoint::idAFConstraint_UniversalJoint( const idStr &name, idAFBody *body1, idAFBody *body2 ) { - assert( body1 ); +idAFConstraint_UniversalJoint::idAFConstraint_UniversalJoint(const idStr& name, idAFBody* body1, idAFBody* body2) { + assert(body1); type = CONSTRAINT_UNIVERSALJOINT; this->name = name; this->body1 = body1; this->body2 = body2; - InitSize( 4 ); + InitSize(4); coneLimit = NULL; pyramidLimit = NULL; friction = 0.0f; @@ -936,14 +936,14 @@ idAFConstraint_UniversalJoint::idAFConstraint_UniversalJoint( const idStr &name, idAFConstraint_UniversalJoint::~idAFConstraint_UniversalJoint ================ */ -idAFConstraint_UniversalJoint::~idAFConstraint_UniversalJoint( void ) { - if ( coneLimit ) { +idAFConstraint_UniversalJoint::~idAFConstraint_UniversalJoint(void) { + if (coneLimit) { delete coneLimit; } - if ( pyramidLimit ) { + if (pyramidLimit) { delete pyramidLimit; } - if ( fc ) { + if (fc) { delete fc; } } @@ -953,23 +953,23 @@ idAFConstraint_UniversalJoint::~idAFConstraint_UniversalJoint( void ) { idAFConstraint_UniversalJoint::SetAnchor ================ */ -void idAFConstraint_UniversalJoint::SetAnchor( const idVec3 &worldPosition ) { +void idAFConstraint_UniversalJoint::SetAnchor(const idVec3& worldPosition) { // get anchor relative to center of mass of body1 - anchor1 = ( worldPosition - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose(); - if ( body2 ) { + anchor1 = (worldPosition - body1->GetWorldOrigin()) * body1->GetWorldAxis().Transpose(); + if (body2) { // get anchor relative to center of mass of body2 - anchor2 = ( worldPosition - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose(); + anchor2 = (worldPosition - body2->GetWorldOrigin()) * body2->GetWorldAxis().Transpose(); } else { anchor2 = worldPosition; } - if ( coneLimit ) { - coneLimit->SetAnchor( anchor2 ); + if (coneLimit) { + coneLimit->SetAnchor(anchor2); } - if ( pyramidLimit ) { - pyramidLimit->SetAnchor( anchor2 ); + if (pyramidLimit) { + pyramidLimit->SetAnchor(anchor2); } } @@ -978,8 +978,8 @@ void idAFConstraint_UniversalJoint::SetAnchor( const idVec3 &worldPosition ) { idAFConstraint_UniversalJoint::GetAnchor ================ */ -idVec3 idAFConstraint_UniversalJoint::GetAnchor( void ) const { - if ( body2 ) { +idVec3 idAFConstraint_UniversalJoint::GetAnchor(void) const { + if (body2) { return body2->GetWorldOrigin() + body2->GetWorldAxis() * anchor2; } return anchor2; @@ -990,28 +990,28 @@ idVec3 idAFConstraint_UniversalJoint::GetAnchor( void ) const { idAFConstraint_UniversalJoint::SetShafts ================ */ -void idAFConstraint_UniversalJoint::SetShafts( const idVec3 &cardanShaft1, const idVec3 &cardanShaft2 ) { +void idAFConstraint_UniversalJoint::SetShafts(const idVec3& cardanShaft1, const idVec3& cardanShaft2) { idVec3 cardanAxis; float l; shaft1 = cardanShaft1; l = shaft1.Normalize(); - assert( l != 0.0f ); + assert(l != 0.0f); shaft2 = cardanShaft2; l = shaft2.Normalize(); - assert( l != 0.0f ); + assert(l != 0.0f); // the cardan axis is a vector orthogonal to both cardan shafts - cardanAxis = shaft1.Cross( shaft2 ); - if ( cardanAxis.Normalize() == 0.0f ) { + cardanAxis = shaft1.Cross(shaft2); + if (cardanAxis.Normalize() == 0.0f) { idVec3 vecY; - shaft1.OrthogonalBasis( cardanAxis, vecY ); + shaft1.OrthogonalBasis(cardanAxis, vecY); cardanAxis.Normalize(); } shaft1 *= body1->GetWorldAxis().Transpose(); axis1 = cardanAxis * body1->GetWorldAxis().Transpose(); - if ( body2 ) { + if (body2) { shaft2 *= body2->GetWorldAxis().Transpose(); axis2 = cardanAxis * body2->GetWorldAxis().Transpose(); } @@ -1019,11 +1019,11 @@ void idAFConstraint_UniversalJoint::SetShafts( const idVec3 &cardanShaft1, const axis2 = cardanAxis; } - if ( coneLimit ) { - coneLimit->SetBody1Axis( shaft1 ); + if (coneLimit) { + coneLimit->SetBody1Axis(shaft1); } - if ( pyramidLimit ) { - pyramidLimit->SetBody1Axis( shaft1 ); + if (pyramidLimit) { + pyramidLimit->SetBody1Axis(shaft1); } } @@ -1032,12 +1032,12 @@ void idAFConstraint_UniversalJoint::SetShafts( const idVec3 &cardanShaft1, const idAFConstraint_UniversalJoint::SetNoLimit ================ */ -void idAFConstraint_UniversalJoint::SetNoLimit( void ) { - if ( coneLimit ) { +void idAFConstraint_UniversalJoint::SetNoLimit(void) { + if (coneLimit) { delete coneLimit; coneLimit = NULL; } - if ( pyramidLimit ) { + if (pyramidLimit) { delete pyramidLimit; pyramidLimit = NULL; } @@ -1048,20 +1048,20 @@ void idAFConstraint_UniversalJoint::SetNoLimit( void ) { idAFConstraint_UniversalJoint::SetConeLimit ================ */ -void idAFConstraint_UniversalJoint::SetConeLimit( const idVec3 &coneAxis, const float coneAngle ) { - if ( pyramidLimit ) { +void idAFConstraint_UniversalJoint::SetConeLimit(const idVec3& coneAxis, const float coneAngle) { + if (pyramidLimit) { delete pyramidLimit; pyramidLimit = NULL; } - if ( !coneLimit ) { + if (!coneLimit) { coneLimit = new idAFConstraint_ConeLimit; - coneLimit->SetPhysics( physics ); + coneLimit->SetPhysics(physics); } - if ( body2 ) { - coneLimit->Setup( body1, body2, anchor2, coneAxis * body2->GetWorldAxis().Transpose(), coneAngle, shaft1 ); + if (body2) { + coneLimit->Setup(body1, body2, anchor2, coneAxis * body2->GetWorldAxis().Transpose(), coneAngle, shaft1); } else { - coneLimit->Setup( body1, body2, anchor2, coneAxis, coneAngle, shaft1 ); + coneLimit->Setup(body1, body2, anchor2, coneAxis, coneAngle, shaft1); } } @@ -1070,22 +1070,22 @@ void idAFConstraint_UniversalJoint::SetConeLimit( const idVec3 &coneAxis, const idAFConstraint_UniversalJoint::SetPyramidLimit ================ */ -void idAFConstraint_UniversalJoint::SetPyramidLimit( const idVec3 &pyramidAxis, const idVec3 &baseAxis, - const float angle1, const float angle2 ) { - if ( coneLimit ) { +void idAFConstraint_UniversalJoint::SetPyramidLimit(const idVec3& pyramidAxis, const idVec3& baseAxis, + const float angle1, const float angle2) { + if (coneLimit) { delete coneLimit; coneLimit = NULL; } - if ( !pyramidLimit ) { + if (!pyramidLimit) { pyramidLimit = new idAFConstraint_PyramidLimit; - pyramidLimit->SetPhysics( physics ); + pyramidLimit->SetPhysics(physics); } - if ( body2 ) { - pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis * body2->GetWorldAxis().Transpose(), - baseAxis * body2->GetWorldAxis().Transpose(), angle1, angle2, shaft1 ); + if (body2) { + pyramidLimit->Setup(body1, body2, anchor2, pyramidAxis * body2->GetWorldAxis().Transpose(), + baseAxis * body2->GetWorldAxis().Transpose(), angle1, angle2, shaft1); } else { - pyramidLimit->Setup( body1, body2, anchor2, pyramidAxis, baseAxis, angle1, angle2, shaft1 ); + pyramidLimit->Setup(body1, body2, anchor2, pyramidAxis, baseAxis, angle1, angle2, shaft1); } } @@ -1094,12 +1094,12 @@ void idAFConstraint_UniversalJoint::SetPyramidLimit( const idVec3 &pyramidAxis, idAFConstraint_UniversalJoint::SetLimitEpsilon ================ */ -void idAFConstraint_UniversalJoint::SetLimitEpsilon( const float e ) { - if ( coneLimit ) { - coneLimit->SetEpsilon( e ); +void idAFConstraint_UniversalJoint::SetLimitEpsilon(const float e) { + if (coneLimit) { + coneLimit->SetEpsilon(e); } - if ( pyramidLimit ) { - pyramidLimit->SetEpsilon( e ); + if (pyramidLimit) { + pyramidLimit->SetEpsilon(e); } } @@ -1108,8 +1108,8 @@ void idAFConstraint_UniversalJoint::SetLimitEpsilon( const float e ) { idAFConstraint_UniversalJoint::GetFriction ================ */ -float idAFConstraint_UniversalJoint::GetFriction( void ) const { - if ( af_forceFriction.GetFloat() > 0.0f ) { +float idAFConstraint_UniversalJoint::GetFriction(void) const { + if (af_forceFriction.GetFloat() > 0.0f) { return af_forceFriction.GetFloat(); } return friction * physics->GetJointFrictionScale(); @@ -1122,70 +1122,70 @@ idAFConstraint_UniversalJoint::Evaluate NOTE: this joint is homokinetic ================ */ -void idAFConstraint_UniversalJoint::Evaluate( float invTimeStep ) { +void idAFConstraint_UniversalJoint::Evaluate(float invTimeStep) { idVec3 a1, a2, s1, s2, d1, d2, v; - idAFBody *master; + idAFBody* master; master = body2 ? body2 : physics->GetMasterBody(); a1 = anchor1 * body1->GetWorldAxis(); s1 = shaft1 * body1->GetWorldAxis(); - d1 = s1.Cross( axis1 * body1->GetWorldAxis() ); + d1 = s1.Cross(axis1 * body1->GetWorldAxis()); - if ( master ) { + if (master) { a2 = anchor2 * master->GetWorldAxis(); s2 = shaft2 * master->GetWorldAxis(); d2 = axis2 * master->GetWorldAxis(); - c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 + master->GetWorldOrigin() - ( a1 + body1->GetWorldOrigin() ) ); + c1.SubVec3(0) = -(invTimeStep * ERROR_REDUCTION) * (a2 + master->GetWorldOrigin() - (a1 + body1->GetWorldOrigin())); } else { a2 = anchor2; s2 = shaft2; d2 = axis2; - c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 - ( a1 + body1->GetWorldOrigin() ) ); + c1.SubVec3(0) = -(invTimeStep * ERROR_REDUCTION) * (a2 - (a1 + body1->GetWorldOrigin())); } - J1.Set( mat3_identity, -SkewSymmetric( a1 ), - mat3_zero, idMat3( s1[0], s1[1], s1[2], - 0.0f, 0.0f, 0.0f, - 0.0f, 0.0f, 0.0f ) ); - J1.SetSize( 4, 6 ); + J1.Set(mat3_identity, -SkewSymmetric(a1), + mat3_zero, idMat3(s1[0], s1[1], s1[2], + 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 0.0f)); + J1.SetSize(4, 6); - if ( body2 ) { - J2.Set( -mat3_identity, SkewSymmetric( a2 ), - mat3_zero, idMat3( s2[0], s2[1], s2[2], - 0.0f, 0.0f, 0.0f, - 0.0f, 0.0f, 0.0f ) ); - J2.SetSize( 4, 6 ); + if (body2) { + J2.Set(-mat3_identity, SkewSymmetric(a2), + mat3_zero, idMat3(s2[0], s2[1], s2[2], + 0.0f, 0.0f, 0.0f, + 0.0f, 0.0f, 0.0f)); + J2.SetSize(4, 6); } else { - J2.Zero( 4, 6 ); + J2.Zero(4, 6); } - v = s1.Cross( s2 ); - if ( v.Normalize() != 0.0f ) { + v = s1.Cross(s2); + if (v.Normalize() != 0.0f) { idMat3 m1, m2; m1[0] = s1; m1[1] = v; - m1[2] = v.Cross( m1[0] ); + m1[2] = v.Cross(m1[0]); m2[0] = -s2; m2[1] = v; - m2[2] = v.Cross( m2[0] ); + m2[2] = v.Cross(m2[0]); d2 *= m2.Transpose() * m1; } - c1[3] = -( invTimeStep * ERROR_REDUCTION ) * ( d1 * d2 ); + c1[3] = -(invTimeStep * ERROR_REDUCTION) * (d1 * d2); - c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX ); + c1.Clamp(-ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX); - if ( coneLimit ) { - coneLimit->Add( physics, invTimeStep ); + if (coneLimit) { + coneLimit->Add(physics, invTimeStep); } - else if ( pyramidLimit ) { - pyramidLimit->Add( physics, invTimeStep ); + else if (pyramidLimit) { + pyramidLimit->Add(physics, invTimeStep); } } @@ -1194,39 +1194,39 @@ void idAFConstraint_UniversalJoint::Evaluate( float invTimeStep ) { idAFConstraint_UniversalJoint::ApplyFriction ================ */ -void idAFConstraint_UniversalJoint::ApplyFriction( float invTimeStep ) { +void idAFConstraint_UniversalJoint::ApplyFriction(float invTimeStep) { idVec3 angular; float invMass, currentFriction; currentFriction = GetFriction(); - if ( currentFriction <= 0.0f ) { + if (currentFriction <= 0.0f) { return; } - if ( af_useImpulseFriction.GetBool() || af_useJointImpulseFriction.GetBool() ) { + if (af_useImpulseFriction.GetBool() || af_useJointImpulseFriction.GetBool()) { angular = body1->GetAngularVelocity(); invMass = body1->GetInverseMass(); - if ( body2 ) { + if (body2) { angular -= body2->GetAngularVelocity(); invMass += body2->GetInverseMass(); } angular *= currentFriction / invMass; - body1->SetAngularVelocity( body1->GetAngularVelocity() - angular * body1->GetInverseMass() ); - if ( body2 ) { - body2->SetAngularVelocity( body2->GetAngularVelocity() + angular * body2->GetInverseMass() ); + body1->SetAngularVelocity(body1->GetAngularVelocity() - angular * body1->GetInverseMass()); + if (body2) { + body2->SetAngularVelocity(body2->GetAngularVelocity() + angular * body2->GetInverseMass()); } } else { - if ( !fc ) { + if (!fc) { fc = new idAFConstraint_UniversalJointFriction; - fc->Setup( this ); + fc->Setup(this); } - fc->Add( physics, invTimeStep ); + fc->Add(physics, invTimeStep); } } @@ -1235,8 +1235,8 @@ void idAFConstraint_UniversalJoint::ApplyFriction( float invTimeStep ) { idAFConstraint_UniversalJoint::GetForce ================ */ -void idAFConstraint_UniversalJoint::GetForce( idAFBody *body, idVec6 &force ) { - idAFConstraint::GetForce( body, force ); +void idAFConstraint_UniversalJoint::GetForce(idAFBody* body, idVec6& force) { + idAFConstraint::GetForce(body, force); // FIXME: add limit force } @@ -1245,15 +1245,15 @@ void idAFConstraint_UniversalJoint::GetForce( idAFBody *body, idVec6 &force ) { idAFConstraint_UniversalJoint::Translate ================ */ -void idAFConstraint_UniversalJoint::Translate( const idVec3 &translation ) { - if ( !body2 ) { +void idAFConstraint_UniversalJoint::Translate(const idVec3& translation) { + if (!body2) { anchor2 += translation; } - if ( coneLimit ) { - coneLimit->Translate( translation ); + if (coneLimit) { + coneLimit->Translate(translation); } - else if ( pyramidLimit ) { - pyramidLimit->Translate( translation ); + else if (pyramidLimit) { + pyramidLimit->Translate(translation); } } @@ -1262,17 +1262,17 @@ void idAFConstraint_UniversalJoint::Translate( const idVec3 &translation ) { idAFConstraint_UniversalJoint::Rotate ================ */ -void idAFConstraint_UniversalJoint::Rotate( const idRotation &rotation ) { - if ( !body2 ) { +void idAFConstraint_UniversalJoint::Rotate(const idRotation& rotation) { + if (!body2) { anchor2 *= rotation; shaft2 *= rotation.ToMat3(); axis2 *= rotation.ToMat3(); } - if ( coneLimit ) { - coneLimit->Rotate( rotation ); + if (coneLimit) { + coneLimit->Rotate(rotation); } - else if ( pyramidLimit ) { - pyramidLimit->Rotate( rotation ); + else if (pyramidLimit) { + pyramidLimit->Rotate(rotation); } } @@ -1281,7 +1281,7 @@ void idAFConstraint_UniversalJoint::Rotate( const idRotation &rotation ) { idAFConstraint_UniversalJoint::GetCenter ================ */ -void idAFConstraint_UniversalJoint::GetCenter( idVec3 ¢er ) { +void idAFConstraint_UniversalJoint::GetCenter(idVec3& center) { center = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); } @@ -1290,9 +1290,9 @@ void idAFConstraint_UniversalJoint::GetCenter( idVec3 ¢er ) { idAFConstraint_UniversalJoint::DebugDraw ================ */ -void idAFConstraint_UniversalJoint::DebugDraw( void ) { +void idAFConstraint_UniversalJoint::DebugDraw(void) { idVec3 a1, a2, s1, s2, d1, d2, v; - idAFBody *master; + idAFBody* master; master = body2 ? body2 : physics->GetMasterBody(); @@ -1300,42 +1300,42 @@ void idAFConstraint_UniversalJoint::DebugDraw( void ) { s1 = shaft1 * body1->GetWorldAxis(); d1 = axis1 * body1->GetWorldAxis(); - if ( master ) { - a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis(); + if (master) { + a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis(); s2 = shaft2 * master->GetWorldAxis(); d2 = axis2 * master->GetWorldAxis(); } else { - a2 = anchor2; + a2 = anchor2; s2 = shaft2; d2 = axis2; } - v = s1.Cross( s2 ); - if ( v.Normalize() != 0.0f ) { + v = s1.Cross(s2); + if (v.Normalize() != 0.0f) { idMat3 m1, m2; m1[0] = s1; m1[1] = v; - m1[2] = v.Cross( m1[0] ); + m1[2] = v.Cross(m1[0]); m2[0] = -s2; m2[1] = v; - m2[2] = v.Cross( m2[0] ); + m2[2] = v.Cross(m2[0]); d2 *= m2.Transpose() * m1; } - gameRenderWorld->DebugArrow( colorCyan, a1, a1 + s1 * 5.0f, 1.0f ); - gameRenderWorld->DebugArrow( colorBlue, a2, a2 + s2 * 5.0f, 1.0f ); - gameRenderWorld->DebugLine( colorGreen, a1, a1 + d1 * 5.0f ); - gameRenderWorld->DebugLine( colorGreen, a2, a2 + d2 * 5.0f ); + gameRenderWorld->DebugArrow(colorCyan, a1, a1 + s1 * 5.0f, 1.0f); + gameRenderWorld->DebugArrow(colorBlue, a2, a2 + s2 * 5.0f, 1.0f); + gameRenderWorld->DebugLine(colorGreen, a1, a1 + d1 * 5.0f); + gameRenderWorld->DebugLine(colorGreen, a2, a2 + d2 * 5.0f); - if ( af_showLimits.GetBool() ) { - if ( coneLimit ) { + if (af_showLimits.GetBool()) { + if (coneLimit) { coneLimit->DebugDraw(); } - if ( pyramidLimit ) { + if (pyramidLimit) { pyramidLimit->DebugDraw(); } } @@ -1346,20 +1346,20 @@ void idAFConstraint_UniversalJoint::DebugDraw( void ) { idAFConstraint_UniversalJoint::Save ================ */ -void idAFConstraint_UniversalJoint::Save( idSaveGame *saveFile ) const { - idAFConstraint::Save( saveFile ); - saveFile->WriteVec3( anchor1 ); - saveFile->WriteVec3( anchor2 ); - saveFile->WriteVec3( shaft1 ); - saveFile->WriteVec3( shaft2 ); - saveFile->WriteVec3( axis1 ); - saveFile->WriteVec3( axis2 ); - saveFile->WriteFloat( friction ); - if ( coneLimit ) { - coneLimit->Save( saveFile ); +void idAFConstraint_UniversalJoint::Save(idSaveGame* saveFile) const { + idAFConstraint::Save(saveFile); + saveFile->WriteVec3(anchor1); + saveFile->WriteVec3(anchor2); + saveFile->WriteVec3(shaft1); + saveFile->WriteVec3(shaft2); + saveFile->WriteVec3(axis1); + saveFile->WriteVec3(axis2); + saveFile->WriteFloat(friction); + if (coneLimit) { + coneLimit->Save(saveFile); } - if ( pyramidLimit ) { - pyramidLimit->Save( saveFile ); + if (pyramidLimit) { + pyramidLimit->Save(saveFile); } } @@ -1368,20 +1368,20 @@ void idAFConstraint_UniversalJoint::Save( idSaveGame *saveFile ) const { idAFConstraint_UniversalJoint::Restore ================ */ -void idAFConstraint_UniversalJoint::Restore( idRestoreGame *saveFile ) { - idAFConstraint::Restore( saveFile ); - saveFile->ReadVec3( anchor1 ); - saveFile->ReadVec3( anchor2 ); - saveFile->ReadVec3( shaft1 ); - saveFile->ReadVec3( shaft2 ); - saveFile->ReadVec3( axis1 ); - saveFile->ReadVec3( axis2 ); - saveFile->ReadFloat( friction ); - if ( coneLimit ) { - coneLimit->Restore( saveFile ); +void idAFConstraint_UniversalJoint::Restore(idRestoreGame* saveFile) { + idAFConstraint::Restore(saveFile); + saveFile->ReadVec3(anchor1); + saveFile->ReadVec3(anchor2); + saveFile->ReadVec3(shaft1); + saveFile->ReadVec3(shaft2); + saveFile->ReadVec3(axis1); + saveFile->ReadVec3(axis2); + saveFile->ReadFloat(friction); + if (coneLimit) { + coneLimit->Restore(saveFile); } - if ( pyramidLimit ) { - pyramidLimit->Restore( saveFile ); + if (pyramidLimit) { + pyramidLimit->Restore(saveFile); } } @@ -1397,10 +1397,10 @@ void idAFConstraint_UniversalJoint::Restore( idRestoreGame *saveFile ) { idAFConstraint_UniversalJointFriction::idAFConstraint_UniversalJointFriction ================ */ -idAFConstraint_UniversalJointFriction::idAFConstraint_UniversalJointFriction( void ) { +idAFConstraint_UniversalJointFriction::idAFConstraint_UniversalJointFriction(void) { type = CONSTRAINT_FRICTION; name = "universalJointFriction"; - InitSize( 2 ); + InitSize(2); joint = NULL; fl.allowPrimary = false; fl.frameConstraint = true; @@ -1411,7 +1411,7 @@ idAFConstraint_UniversalJointFriction::idAFConstraint_UniversalJointFriction( vo idAFConstraint_UniversalJointFriction::Setup ================ */ -void idAFConstraint_UniversalJointFriction::Setup( idAFConstraint_UniversalJoint *uj ) { +void idAFConstraint_UniversalJointFriction::Setup(idAFConstraint_UniversalJoint* uj) { this->joint = uj; body1 = uj->GetBody1(); body2 = uj->GetBody2(); @@ -1422,7 +1422,7 @@ void idAFConstraint_UniversalJointFriction::Setup( idAFConstraint_UniversalJoint idAFConstraint_UniversalJointFriction::Evaluate ================ */ -void idAFConstraint_UniversalJointFriction::Evaluate( float invTimeStep ) { +void idAFConstraint_UniversalJointFriction::Evaluate(float invTimeStep) { // do nothing } @@ -1431,7 +1431,7 @@ void idAFConstraint_UniversalJointFriction::Evaluate( float invTimeStep ) { idAFConstraint_UniversalJointFriction::ApplyFriction ================ */ -void idAFConstraint_UniversalJointFriction::ApplyFriction( float invTimeStep ) { +void idAFConstraint_UniversalJointFriction::ApplyFriction(float invTimeStep) { // do nothing } @@ -1440,41 +1440,41 @@ void idAFConstraint_UniversalJointFriction::ApplyFriction( float invTimeStep ) { idAFConstraint_UniversalJointFriction::Add ================ */ -bool idAFConstraint_UniversalJointFriction::Add( idPhysics_AF *phys, float invTimeStep ) { +bool idAFConstraint_UniversalJointFriction::Add(idPhysics_AF* phys, float invTimeStep) { idVec3 s1, s2, dir1, dir2; float f; physics = phys; f = joint->GetFriction() * joint->GetMultiplier().Length(); - if ( f == 0.0f ) { + if (f == 0.0f) { return false; } lo[0] = lo[1] = -f; hi[0] = hi[1] = f; - joint->GetShafts( s1, s2 ); + joint->GetShafts(s1, s2); s1 *= body1->GetWorldAxis(); - s1.NormalVectors( dir1, dir2 ); + s1.NormalVectors(dir1, dir2); - J1.SetSize( 2, 6 ); + J1.SetSize(2, 6); J1.SubVec6(0).SubVec3(0).Zero(); J1.SubVec6(0).SubVec3(1) = dir1; J1.SubVec6(1).SubVec3(0).Zero(); J1.SubVec6(1).SubVec3(1) = dir2; - if ( body2 ) { + if (body2) { - J2.SetSize( 2, 6 ); + J2.SetSize(2, 6); J2.SubVec6(0).SubVec3(0).Zero(); J2.SubVec6(0).SubVec3(1) = -dir1; J2.SubVec6(1).SubVec3(0).Zero(); J2.SubVec6(1).SubVec3(1) = -dir2; } - physics->AddFrameConstraint( this ); + physics->AddFrameConstraint(this); return true; } @@ -1484,7 +1484,7 @@ bool idAFConstraint_UniversalJointFriction::Add( idPhysics_AF *phys, float invTi idAFConstraint_UniversalJointFriction::Translate ================ */ -void idAFConstraint_UniversalJointFriction::Translate( const idVec3 &translation ) { +void idAFConstraint_UniversalJointFriction::Translate(const idVec3& translation) { } /* @@ -1492,7 +1492,7 @@ void idAFConstraint_UniversalJointFriction::Translate( const idVec3 &translation idAFConstraint_UniversalJointFriction::Rotate ================ */ -void idAFConstraint_UniversalJointFriction::Rotate( const idRotation &rotation ) { +void idAFConstraint_UniversalJointFriction::Rotate(const idRotation& rotation) { } @@ -1507,8 +1507,8 @@ void idAFConstraint_UniversalJointFriction::Rotate( const idRotation &rotation ) idAFConstraint_CylindricalJoint::idAFConstraint_CylindricalJoint ================ */ -idAFConstraint_CylindricalJoint::idAFConstraint_CylindricalJoint( const idStr &name, idAFBody *body1, idAFBody *body2 ) { - assert( 0 ); // FIXME: implement +idAFConstraint_CylindricalJoint::idAFConstraint_CylindricalJoint(const idStr& name, idAFBody* body1, idAFBody* body2) { + assert(0); // FIXME: implement } /* @@ -1516,8 +1516,8 @@ idAFConstraint_CylindricalJoint::idAFConstraint_CylindricalJoint( const idStr &n idAFConstraint_CylindricalJoint::Evaluate ================ */ -void idAFConstraint_CylindricalJoint::Evaluate( float invTimeStep ) { - assert( 0 ); // FIXME: implement +void idAFConstraint_CylindricalJoint::Evaluate(float invTimeStep) { + assert(0); // FIXME: implement } /* @@ -1525,8 +1525,8 @@ void idAFConstraint_CylindricalJoint::Evaluate( float invTimeStep ) { idAFConstraint_CylindricalJoint::ApplyFriction ================ */ -void idAFConstraint_CylindricalJoint::ApplyFriction( float invTimeStep ) { - assert( 0 ); // FIXME: implement +void idAFConstraint_CylindricalJoint::ApplyFriction(float invTimeStep) { + assert(0); // FIXME: implement } /* @@ -1534,8 +1534,8 @@ void idAFConstraint_CylindricalJoint::ApplyFriction( float invTimeStep ) { idAFConstraint_CylindricalJoint::Translate ================ */ -void idAFConstraint_CylindricalJoint::Translate( const idVec3 &translation ) { - assert( 0 ); // FIXME: implement +void idAFConstraint_CylindricalJoint::Translate(const idVec3& translation) { + assert(0); // FIXME: implement } /* @@ -1543,8 +1543,8 @@ void idAFConstraint_CylindricalJoint::Translate( const idVec3 &translation ) { idAFConstraint_CylindricalJoint::Rotate ================ */ -void idAFConstraint_CylindricalJoint::Rotate( const idRotation &rotation ) { - assert( 0 ); // FIXME: implement +void idAFConstraint_CylindricalJoint::Rotate(const idRotation& rotation) { + assert(0); // FIXME: implement } /* @@ -1552,8 +1552,8 @@ void idAFConstraint_CylindricalJoint::Rotate( const idRotation &rotation ) { idAFConstraint_CylindricalJoint::DebugDraw ================ */ -void idAFConstraint_CylindricalJoint::DebugDraw( void ) { - assert( 0 ); // FIXME: implement +void idAFConstraint_CylindricalJoint::DebugDraw(void) { + assert(0); // FIXME: implement } @@ -1568,13 +1568,13 @@ void idAFConstraint_CylindricalJoint::DebugDraw( void ) { idAFConstraint_Hinge::idAFConstraint_Hinge ================ */ -idAFConstraint_Hinge::idAFConstraint_Hinge( const idStr &name, idAFBody *body1, idAFBody *body2 ) { - assert( body1 ); +idAFConstraint_Hinge::idAFConstraint_Hinge(const idStr& name, idAFBody* body1, idAFBody* body2) { + assert(body1); type = CONSTRAINT_HINGE; this->name = name; this->body1 = body1; this->body2 = body2; - InitSize( 5 ); + InitSize(5); coneLimit = NULL; steering = NULL; friction = 0.0f; @@ -1582,7 +1582,7 @@ idAFConstraint_Hinge::idAFConstraint_Hinge( const idStr &name, idAFBody *body1, fl.allowPrimary = true; fl.noCollision = true; initialAxis = body1->GetWorldAxis(); - if ( body2 ) { + if (body2) { initialAxis *= body2->GetWorldAxis().Transpose(); } } @@ -1592,14 +1592,14 @@ idAFConstraint_Hinge::idAFConstraint_Hinge( const idStr &name, idAFBody *body1, idAFConstraint_Hinge::~idAFConstraint_Hinge ================ */ -idAFConstraint_Hinge::~idAFConstraint_Hinge( void ) { - if ( coneLimit ) { +idAFConstraint_Hinge::~idAFConstraint_Hinge(void) { + if (coneLimit) { delete coneLimit; } - if ( fc ) { + if (fc) { delete fc; } - if ( steering ) { + if (steering) { delete steering; } } @@ -1609,19 +1609,19 @@ idAFConstraint_Hinge::~idAFConstraint_Hinge( void ) { idAFConstraint_Hinge::SetAnchor ================ */ -void idAFConstraint_Hinge::SetAnchor( const idVec3 &worldPosition ) { +void idAFConstraint_Hinge::SetAnchor(const idVec3& worldPosition) { // get anchor relative to center of mass of body1 - anchor1 = ( worldPosition - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose(); - if ( body2 ) { + anchor1 = (worldPosition - body1->GetWorldOrigin()) * body1->GetWorldAxis().Transpose(); + if (body2) { // get anchor relative to center of mass of body2 - anchor2 = ( worldPosition - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose(); + anchor2 = (worldPosition - body2->GetWorldOrigin()) * body2->GetWorldAxis().Transpose(); } else { anchor2 = worldPosition; } - if ( coneLimit ) { - coneLimit->SetAnchor( anchor2 ); + if (coneLimit) { + coneLimit->SetAnchor(anchor2); } } @@ -1630,8 +1630,8 @@ void idAFConstraint_Hinge::SetAnchor( const idVec3 &worldPosition ) { idAFConstraint_Hinge::GetAnchor ================ */ -idVec3 idAFConstraint_Hinge::GetAnchor( void ) const { - if ( body2 ) { +idVec3 idAFConstraint_Hinge::GetAnchor(void) const { + if (body2) { return body2->GetWorldOrigin() + body2->GetWorldAxis() * anchor2; } return anchor2; @@ -1642,7 +1642,7 @@ idVec3 idAFConstraint_Hinge::GetAnchor( void ) const { idAFConstraint_Hinge::SetAxis ================ */ -void idAFConstraint_Hinge::SetAxis( const idVec3 &axis ) { +void idAFConstraint_Hinge::SetAxis(const idVec3& axis) { idVec3 normAxis; normAxis = axis; @@ -1650,7 +1650,7 @@ void idAFConstraint_Hinge::SetAxis( const idVec3 &axis ) { // get axis relative to body1 axis1 = normAxis * body1->GetWorldAxis().Transpose(); - if ( body2 ) { + if (body2) { // get axis relative to body2 axis2 = normAxis * body2->GetWorldAxis().Transpose(); } @@ -1664,8 +1664,8 @@ void idAFConstraint_Hinge::SetAxis( const idVec3 &axis ) { idAFConstraint_Hinge::GetAxis ================ */ -idVec3 idAFConstraint_Hinge::GetAxis( void ) const { - if ( body2 ) { +idVec3 idAFConstraint_Hinge::GetAxis(void) const { + if (body2) { return axis2 * body2->GetWorldAxis(); } return axis2; @@ -1676,8 +1676,8 @@ idVec3 idAFConstraint_Hinge::GetAxis( void ) const { idAFConstraint_Hinge::SetNoLimit ================ */ -void idAFConstraint_Hinge::SetNoLimit( void ) { - if ( coneLimit ) { +void idAFConstraint_Hinge::SetNoLimit(void) { + if (coneLimit) { delete coneLimit; coneLimit = NULL; } @@ -1688,16 +1688,16 @@ void idAFConstraint_Hinge::SetNoLimit( void ) { idAFConstraint_Hinge::SetLimit ================ */ -void idAFConstraint_Hinge::SetLimit( const idVec3 &axis, const float angle, const idVec3 &body1Axis ) { - if ( !coneLimit ) { +void idAFConstraint_Hinge::SetLimit(const idVec3& axis, const float angle, const idVec3& body1Axis) { + if (!coneLimit) { coneLimit = new idAFConstraint_ConeLimit; - coneLimit->SetPhysics( physics ); + coneLimit->SetPhysics(physics); } - if ( body2 ) { - coneLimit->Setup( body1, body2, anchor2, axis * body2->GetWorldAxis().Transpose(), angle, body1Axis * body1->GetWorldAxis().Transpose() ); + if (body2) { + coneLimit->Setup(body1, body2, anchor2, axis * body2->GetWorldAxis().Transpose(), angle, body1Axis * body1->GetWorldAxis().Transpose()); } else { - coneLimit->Setup( body1, body2, anchor2, axis, angle, body1Axis * body1->GetWorldAxis().Transpose() ); + coneLimit->Setup(body1, body2, anchor2, axis, angle, body1Axis * body1->GetWorldAxis().Transpose()); } } @@ -1706,9 +1706,9 @@ void idAFConstraint_Hinge::SetLimit( const idVec3 &axis, const float angle, cons idAFConstraint_Hinge::SetLimitEpsilon ================ */ -void idAFConstraint_Hinge::SetLimitEpsilon( const float e ) { - if ( coneLimit ) { - coneLimit->SetEpsilon( e ); +void idAFConstraint_Hinge::SetLimitEpsilon(const float e) { + if (coneLimit) { + coneLimit->SetEpsilon(e); } } @@ -1717,8 +1717,8 @@ void idAFConstraint_Hinge::SetLimitEpsilon( const float e ) { idAFConstraint_Hinge::GetFriction ================ */ -float idAFConstraint_Hinge::GetFriction( void ) const { - if ( af_forceFriction.GetFloat() > 0.0f ) { +float idAFConstraint_Hinge::GetFriction(void) const { + if (af_forceFriction.GetFloat() > 0.0f) { return af_forceFriction.GetFloat(); } return friction * physics->GetJointFrictionScale(); @@ -1729,7 +1729,7 @@ float idAFConstraint_Hinge::GetFriction( void ) const { idAFConstraint_Hinge::GetAngle ================ */ -float idAFConstraint_Hinge::GetAngle( void ) const { +float idAFConstraint_Hinge::GetAngle(void) const { idMat3 axis; idRotation rotation; float angle; @@ -1737,7 +1737,7 @@ float idAFConstraint_Hinge::GetAngle( void ) const { axis = body1->GetWorldAxis() * body2->GetWorldAxis().Transpose() * initialAxis.Transpose(); rotation = axis.ToRotation(); angle = rotation.GetAngle(); - if ( rotation.GetVec() * axis1 < 0.0f ) { + if (rotation.GetVec() * axis1 < 0.0f) { return -angle; } return angle; @@ -1748,16 +1748,16 @@ float idAFConstraint_Hinge::GetAngle( void ) const { idAFConstraint_Hinge::SetSteerAngle ================ */ -void idAFConstraint_Hinge::SetSteerAngle( const float degrees ) { - if ( coneLimit ) { +void idAFConstraint_Hinge::SetSteerAngle(const float degrees) { + if (coneLimit) { delete coneLimit; coneLimit = NULL; } - if ( !steering ) { + if (!steering) { steering = new idAFConstraint_HingeSteering(); - steering->Setup( this ); + steering->Setup(this); } - steering->SetSteerAngle( degrees ); + steering->SetSteerAngle(degrees); } /* @@ -1765,9 +1765,9 @@ void idAFConstraint_Hinge::SetSteerAngle( const float degrees ) { idAFConstraint_Hinge::SetSteerSpeed ================ */ -void idAFConstraint_Hinge::SetSteerSpeed( const float speed ) { - if ( steering ) { - steering->SetSteerSpeed( speed ); +void idAFConstraint_Hinge::SetSteerSpeed(const float speed) { + if (steering) { + steering->SetSteerSpeed(speed); } } @@ -1776,59 +1776,59 @@ void idAFConstraint_Hinge::SetSteerSpeed( const float speed ) { idAFConstraint_Hinge::Evaluate ================ */ -void idAFConstraint_Hinge::Evaluate( float invTimeStep ) { +void idAFConstraint_Hinge::Evaluate(float invTimeStep) { idVec3 a1, a2; idVec3 x1, x2, cross; idVec3 vecX, vecY; - idAFBody *master; + idAFBody* master; master = body2 ? body2 : physics->GetMasterBody(); x1 = axis1 * body1->GetWorldAxis(); // axis in body1 space - x1.OrthogonalBasis( vecX, vecY ); // basis for axis in body1 space + x1.OrthogonalBasis(vecX, vecY); // basis for axis in body1 space a1 = anchor1 * body1->GetWorldAxis(); // anchor in body1 space - if ( master ) { + if (master) { a2 = anchor2 * master->GetWorldAxis(); // anchor in master space x2 = axis2 * master->GetWorldAxis(); - c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 + master->GetWorldOrigin() - ( a1 + body1->GetWorldOrigin() ) ); + c1.SubVec3(0) = -(invTimeStep * ERROR_REDUCTION) * (a2 + master->GetWorldOrigin() - (a1 + body1->GetWorldOrigin())); } else { a2 = anchor2; x2 = axis2; - c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( a2 - ( a1 + body1->GetWorldOrigin() ) ); + c1.SubVec3(0) = -(invTimeStep * ERROR_REDUCTION) * (a2 - (a1 + body1->GetWorldOrigin())); } - J1.Set( mat3_identity, -SkewSymmetric( a1 ), - mat3_zero, idMat3( vecX[0], vecX[1], vecX[2], - vecY[0], vecY[1], vecY[2], - 0.0f, 0.0f, 0.0f ) ); - J1.SetSize( 5, 6 ); + J1.Set(mat3_identity, -SkewSymmetric(a1), + mat3_zero, idMat3(vecX[0], vecX[1], vecX[2], + vecY[0], vecY[1], vecY[2], + 0.0f, 0.0f, 0.0f)); + J1.SetSize(5, 6); - if ( body2 ) { - J2.Set( -mat3_identity, SkewSymmetric( a2 ), - mat3_zero, idMat3( -vecX[0], -vecX[1], -vecX[2], - -vecY[0], -vecY[1], -vecY[2], - 0.0f, 0.0f, 0.0f ) ); - J2.SetSize( 5, 6 ); + if (body2) { + J2.Set(-mat3_identity, SkewSymmetric(a2), + mat3_zero, idMat3(-vecX[0], -vecX[1], -vecX[2], + -vecY[0], -vecY[1], -vecY[2], + 0.0f, 0.0f, 0.0f)); + J2.SetSize(5, 6); } else { - J2.Zero( 5, 6 ); + J2.Zero(5, 6); } - cross = x1.Cross( x2 ); + cross = x1.Cross(x2); - c1[3] = -( invTimeStep * ERROR_REDUCTION ) * ( cross * vecX ); - c1[4] = -( invTimeStep * ERROR_REDUCTION ) * ( cross * vecY ); + c1[3] = -(invTimeStep * ERROR_REDUCTION) * (cross * vecX); + c1[4] = -(invTimeStep * ERROR_REDUCTION) * (cross * vecY); - c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX ); + c1.Clamp(-ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX); - if ( steering ) { - steering->Add( physics, invTimeStep ); + if (steering) { + steering->Add(physics, invTimeStep); } - else if ( coneLimit ) { - coneLimit->Add( physics, invTimeStep ); + else if (coneLimit) { + coneLimit->Add(physics, invTimeStep); } } @@ -1837,39 +1837,39 @@ void idAFConstraint_Hinge::Evaluate( float invTimeStep ) { idAFConstraint_Hinge::ApplyFriction ================ */ -void idAFConstraint_Hinge::ApplyFriction( float invTimeStep ) { +void idAFConstraint_Hinge::ApplyFriction(float invTimeStep) { idVec3 angular; float invMass, currentFriction; currentFriction = GetFriction(); - if ( currentFriction <= 0.0f ) { + if (currentFriction <= 0.0f) { return; } - if ( af_useImpulseFriction.GetBool() || af_useJointImpulseFriction.GetBool() ) { + if (af_useImpulseFriction.GetBool() || af_useJointImpulseFriction.GetBool()) { angular = body1->GetAngularVelocity(); invMass = body1->GetInverseMass(); - if ( body2 ) { + if (body2) { angular -= body2->GetAngularVelocity(); invMass += body2->GetInverseMass(); } angular *= currentFriction / invMass; - body1->SetAngularVelocity( body1->GetAngularVelocity() - angular * body1->GetInverseMass() ); - if ( body2 ) { - body2->SetAngularVelocity( body2->GetAngularVelocity() + angular * body2->GetInverseMass() ); + body1->SetAngularVelocity(body1->GetAngularVelocity() - angular * body1->GetInverseMass()); + if (body2) { + body2->SetAngularVelocity(body2->GetAngularVelocity() + angular * body2->GetInverseMass()); } } else { - if ( !fc ) { + if (!fc) { fc = new idAFConstraint_HingeFriction; - fc->Setup( this ); + fc->Setup(this); } - fc->Add( physics, invTimeStep ); + fc->Add(physics, invTimeStep); } } @@ -1878,8 +1878,8 @@ void idAFConstraint_Hinge::ApplyFriction( float invTimeStep ) { idAFConstraint_Hinge::GetForce ================ */ -void idAFConstraint_Hinge::GetForce( idAFBody *body, idVec6 &force ) { - idAFConstraint::GetForce( body, force ); +void idAFConstraint_Hinge::GetForce(idAFBody* body, idVec6& force) { + idAFConstraint::GetForce(body, force); // FIXME: add limit force } @@ -1888,12 +1888,12 @@ void idAFConstraint_Hinge::GetForce( idAFBody *body, idVec6 &force ) { idAFConstraint_Hinge::Translate ================ */ -void idAFConstraint_Hinge::Translate( const idVec3 &translation ) { - if ( !body2 ) { +void idAFConstraint_Hinge::Translate(const idVec3& translation) { + if (!body2) { anchor2 += translation; } - if ( coneLimit ) { - coneLimit->Translate( translation ); + if (coneLimit) { + coneLimit->Translate(translation); } } @@ -1902,13 +1902,13 @@ void idAFConstraint_Hinge::Translate( const idVec3 &translation ) { idAFConstraint_Hinge::Rotate ================ */ -void idAFConstraint_Hinge::Rotate( const idRotation &rotation ) { - if ( !body2 ) { +void idAFConstraint_Hinge::Rotate(const idRotation& rotation) { + if (!body2) { anchor2 *= rotation; axis2 *= rotation.ToMat3(); } - if ( coneLimit ) { - coneLimit->Rotate( rotation ); + if (coneLimit) { + coneLimit->Rotate(rotation); } } @@ -1917,7 +1917,7 @@ void idAFConstraint_Hinge::Rotate( const idRotation &rotation ) { idAFConstraint_Hinge::GetCenter ================ */ -void idAFConstraint_Hinge::GetCenter( idVec3 ¢er ) { +void idAFConstraint_Hinge::GetCenter(idVec3& center) { center = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); } @@ -1926,18 +1926,18 @@ void idAFConstraint_Hinge::GetCenter( idVec3 ¢er ) { idAFConstraint_Hinge::DebugDraw ================ */ -void idAFConstraint_Hinge::DebugDraw( void ) { +void idAFConstraint_Hinge::DebugDraw(void) { idVec3 vecX, vecY; idVec3 a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); idVec3 x1 = axis1 * body1->GetWorldAxis(); - x1.OrthogonalBasis( vecX, vecY ); + x1.OrthogonalBasis(vecX, vecY); - gameRenderWorld->DebugArrow( colorBlue, a1 - 4.0f * x1, a1 + 4.0f * x1, 1 ); - gameRenderWorld->DebugLine( colorBlue, a1 - 2.0f * vecX, a1 + 2.0f * vecX ); - gameRenderWorld->DebugLine( colorBlue, a1 - 2.0f * vecY, a1 + 2.0f * vecY ); + gameRenderWorld->DebugArrow(colorBlue, a1 - 4.0f * x1, a1 + 4.0f * x1, 1); + gameRenderWorld->DebugLine(colorBlue, a1 - 2.0f * vecX, a1 + 2.0f * vecX); + gameRenderWorld->DebugLine(colorBlue, a1 - 2.0f * vecY, a1 + 2.0f * vecY); - if ( af_showLimits.GetBool() ) { - if ( coneLimit ) { + if (af_showLimits.GetBool()) { + if (coneLimit) { coneLimit->DebugDraw(); } } @@ -1948,31 +1948,34 @@ void idAFConstraint_Hinge::DebugDraw( void ) { idAFConstraint_Hinge::Save ================ */ -void idAFConstraint_Hinge::Save( idSaveGame *saveFile ) const { - idAFConstraint::Save( saveFile ); - saveFile->WriteVec3( anchor1 ); - saveFile->WriteVec3( anchor2 ); - saveFile->WriteVec3( axis1 ); - saveFile->WriteVec3( axis2 ); - saveFile->WriteMat3( initialAxis ); - saveFile->WriteFloat( friction ); - if ( coneLimit ) { - saveFile->WriteBool( true ); - coneLimit->Save( saveFile ); - } else { - saveFile->WriteBool( false ); +void idAFConstraint_Hinge::Save(idSaveGame* saveFile) const { + idAFConstraint::Save(saveFile); + saveFile->WriteVec3(anchor1); + saveFile->WriteVec3(anchor2); + saveFile->WriteVec3(axis1); + saveFile->WriteVec3(axis2); + saveFile->WriteMat3(initialAxis); + saveFile->WriteFloat(friction); + if (coneLimit) { + saveFile->WriteBool(true); + coneLimit->Save(saveFile); } - if ( steering ) { - saveFile->WriteBool( true ); - steering->Save( saveFile ); - } else { - saveFile->WriteBool( false ); + else { + saveFile->WriteBool(false); } - if ( fc ) { - saveFile->WriteBool( true ); - fc->Save( saveFile ); - } else { - saveFile->WriteBool( false ); + if (steering) { + saveFile->WriteBool(true); + steering->Save(saveFile); + } + else { + saveFile->WriteBool(false); + } + if (fc) { + saveFile->WriteBool(true); + fc->Save(saveFile); + } + else { + saveFile->WriteBool(false); } } @@ -1981,39 +1984,39 @@ void idAFConstraint_Hinge::Save( idSaveGame *saveFile ) const { idAFConstraint_Hinge::Restore ================ */ -void idAFConstraint_Hinge::Restore( idRestoreGame *saveFile ) { +void idAFConstraint_Hinge::Restore(idRestoreGame* saveFile) { bool b; - idAFConstraint::Restore( saveFile ); - saveFile->ReadVec3( anchor1 ); - saveFile->ReadVec3( anchor2 ); - saveFile->ReadVec3( axis1 ); - saveFile->ReadVec3( axis2 ); - saveFile->ReadMat3( initialAxis ); - saveFile->ReadFloat( friction ); + idAFConstraint::Restore(saveFile); + saveFile->ReadVec3(anchor1); + saveFile->ReadVec3(anchor2); + saveFile->ReadVec3(axis1); + saveFile->ReadVec3(axis2); + saveFile->ReadMat3(initialAxis); + saveFile->ReadFloat(friction); - saveFile->ReadBool( b ); - if ( b ) { - if ( !coneLimit ) { + saveFile->ReadBool(b); + if (b) { + if (!coneLimit) { coneLimit = new idAFConstraint_ConeLimit; } - coneLimit->SetPhysics( physics ); - coneLimit->Restore( saveFile ); + coneLimit->SetPhysics(physics); + coneLimit->Restore(saveFile); } - saveFile->ReadBool( b ); - if ( b ) { - if ( !steering ) { + saveFile->ReadBool(b); + if (b) { + if (!steering) { steering = new idAFConstraint_HingeSteering; } - steering->Setup( this ); - steering->Restore( saveFile ); + steering->Setup(this); + steering->Restore(saveFile); } - saveFile->ReadBool( b ); - if ( b ) { - if ( !fc ) { + saveFile->ReadBool(b); + if (b) { + if (!fc) { fc = new idAFConstraint_HingeFriction; } - fc->Setup( this ); - fc->Restore( saveFile ); + fc->Setup(this); + fc->Restore(saveFile); } } @@ -2029,10 +2032,10 @@ void idAFConstraint_Hinge::Restore( idRestoreGame *saveFile ) { idAFConstraint_HingeFriction::idAFConstraint_HingeFriction ================ */ -idAFConstraint_HingeFriction::idAFConstraint_HingeFriction( void ) { +idAFConstraint_HingeFriction::idAFConstraint_HingeFriction(void) { type = CONSTRAINT_FRICTION; name = "hingeFriction"; - InitSize( 1 ); + InitSize(1); hinge = NULL; fl.allowPrimary = false; fl.frameConstraint = true; @@ -2043,7 +2046,7 @@ idAFConstraint_HingeFriction::idAFConstraint_HingeFriction( void ) { idAFConstraint_HingeFriction::Setup ================ */ -void idAFConstraint_HingeFriction::Setup( idAFConstraint_Hinge *h ) { +void idAFConstraint_HingeFriction::Setup(idAFConstraint_Hinge* h) { this->hinge = h; body1 = h->GetBody1(); body2 = h->GetBody2(); @@ -2054,7 +2057,7 @@ void idAFConstraint_HingeFriction::Setup( idAFConstraint_Hinge *h ) { idAFConstraint_HingeFriction::Evaluate ================ */ -void idAFConstraint_HingeFriction::Evaluate( float invTimeStep ) { +void idAFConstraint_HingeFriction::Evaluate(float invTimeStep) { // do nothing } @@ -2063,7 +2066,7 @@ void idAFConstraint_HingeFriction::Evaluate( float invTimeStep ) { idAFConstraint_HingeFriction::ApplyFriction ================ */ -void idAFConstraint_HingeFriction::ApplyFriction( float invTimeStep ) { +void idAFConstraint_HingeFriction::ApplyFriction(float invTimeStep) { // do nothing } @@ -2072,37 +2075,37 @@ void idAFConstraint_HingeFriction::ApplyFriction( float invTimeStep ) { idAFConstraint_HingeFriction::Add ================ */ -bool idAFConstraint_HingeFriction::Add( idPhysics_AF *phys, float invTimeStep ) { +bool idAFConstraint_HingeFriction::Add(idPhysics_AF* phys, float invTimeStep) { idVec3 a1, a2; float f; physics = phys; f = hinge->GetFriction() * hinge->GetMultiplier().Length(); - if ( f == 0.0f ) { + if (f == 0.0f) { return false; } lo[0] = -f; hi[0] = f; - hinge->GetAxis( a1, a2 ); + hinge->GetAxis(a1, a2); a1 *= body1->GetWorldAxis(); - J1.SetSize( 1, 6 ); + J1.SetSize(1, 6); J1.SubVec6(0).SubVec3(0).Zero(); J1.SubVec6(0).SubVec3(1) = a1; - if ( body2 ) { + if (body2) { a2 *= body2->GetWorldAxis(); - J2.SetSize( 1, 6 ); + J2.SetSize(1, 6); J2.SubVec6(0).SubVec3(0).Zero(); J2.SubVec6(0).SubVec3(1) = -a2; } - physics->AddFrameConstraint( this ); + physics->AddFrameConstraint(this); return true; } @@ -2112,7 +2115,7 @@ bool idAFConstraint_HingeFriction::Add( idPhysics_AF *phys, float invTimeStep ) idAFConstraint_HingeFriction::Translate ================ */ -void idAFConstraint_HingeFriction::Translate( const idVec3 &translation ) { +void idAFConstraint_HingeFriction::Translate(const idVec3& translation) { } /* @@ -2120,7 +2123,7 @@ void idAFConstraint_HingeFriction::Translate( const idVec3 &translation ) { idAFConstraint_HingeFriction::Rotate ================ */ -void idAFConstraint_HingeFriction::Rotate( const idRotation &rotation ) { +void idAFConstraint_HingeFriction::Rotate(const idRotation& rotation) { } @@ -2135,10 +2138,10 @@ void idAFConstraint_HingeFriction::Rotate( const idRotation &rotation ) { idAFConstraint_HingeSteering::idAFConstraint_HingeSteering ================ */ -idAFConstraint_HingeSteering::idAFConstraint_HingeSteering( void ) { +idAFConstraint_HingeSteering::idAFConstraint_HingeSteering(void) { type = CONSTRAINT_HINGESTEERING; name = "hingeFriction"; - InitSize( 1 ); + InitSize(1); hinge = NULL; fl.allowPrimary = false; fl.frameConstraint = true; @@ -2151,7 +2154,7 @@ idAFConstraint_HingeSteering::idAFConstraint_HingeSteering( void ) { idAFConstraint_HingeSteering::Save ================ */ -void idAFConstraint_HingeSteering::Save( idSaveGame *saveFile ) const { +void idAFConstraint_HingeSteering::Save(idSaveGame* saveFile) const { saveFile->WriteFloat(steerAngle); saveFile->WriteFloat(steerSpeed); saveFile->WriteFloat(epsilon); @@ -2162,7 +2165,7 @@ void idAFConstraint_HingeSteering::Save( idSaveGame *saveFile ) const { idAFConstraint_HingeSteering::Restore ================ */ -void idAFConstraint_HingeSteering::Restore( idRestoreGame *saveFile ) { +void idAFConstraint_HingeSteering::Restore(idRestoreGame* saveFile) { saveFile->ReadFloat(steerAngle); saveFile->ReadFloat(steerSpeed); saveFile->ReadFloat(epsilon); @@ -2173,7 +2176,7 @@ void idAFConstraint_HingeSteering::Restore( idRestoreGame *saveFile ) { idAFConstraint_HingeSteering::Setup ================ */ -void idAFConstraint_HingeSteering::Setup( idAFConstraint_Hinge *h ) { +void idAFConstraint_HingeSteering::Setup(idAFConstraint_Hinge* h) { this->hinge = h; body1 = h->GetBody1(); body2 = h->GetBody2(); @@ -2184,7 +2187,7 @@ void idAFConstraint_HingeSteering::Setup( idAFConstraint_Hinge *h ) { idAFConstraint_HingeSteering::Evaluate ================ */ -void idAFConstraint_HingeSteering::Evaluate( float invTimeStep ) { +void idAFConstraint_HingeSteering::Evaluate(float invTimeStep) { // do nothing } @@ -2193,7 +2196,7 @@ void idAFConstraint_HingeSteering::Evaluate( float invTimeStep ) { idAFConstraint_HingeSteering::ApplyFriction ================ */ -void idAFConstraint_HingeSteering::ApplyFriction( float invTimeStep ) { +void idAFConstraint_HingeSteering::ApplyFriction(float invTimeStep) { // do nothing } @@ -2202,42 +2205,42 @@ void idAFConstraint_HingeSteering::ApplyFriction( float invTimeStep ) { idAFConstraint_HingeSteering::Add ================ */ -bool idAFConstraint_HingeSteering::Add( idPhysics_AF *phys, float invTimeStep ) { +bool idAFConstraint_HingeSteering::Add(idPhysics_AF* phys, float invTimeStep) { float angle, speed; idVec3 a1, a2; physics = phys; - hinge->GetAxis( a1, a2 ); + hinge->GetAxis(a1, a2); angle = hinge->GetAngle(); a1 *= body1->GetWorldAxis(); - J1.SetSize( 1, 6 ); + J1.SetSize(1, 6); J1.SubVec6(0).SubVec3(0).Zero(); J1.SubVec6(0).SubVec3(1) = a1; - if ( body2 ) { + if (body2) { a2 *= body2->GetWorldAxis(); - J2.SetSize( 1, 6 ); + J2.SetSize(1, 6); J2.SubVec6(0).SubVec3(0).Zero(); J2.SubVec6(0).SubVec3(1) = -a2; } speed = steerAngle - angle; - if ( steerSpeed != 0.0f ) { - if ( speed > steerSpeed ) { + if (steerSpeed != 0.0f) { + if (speed > steerSpeed) { speed = steerSpeed; } - else if ( speed < -steerSpeed ) { + else if (speed < -steerSpeed) { speed = -steerSpeed; } } - c1[0] = DEG2RAD( speed ) * invTimeStep; + c1[0] = DEG2RAD(speed) * invTimeStep; - physics->AddFrameConstraint( this ); + physics->AddFrameConstraint(this); return true; } @@ -2247,7 +2250,7 @@ bool idAFConstraint_HingeSteering::Add( idPhysics_AF *phys, float invTimeStep ) idAFConstraint_HingeSteering::Translate ================ */ -void idAFConstraint_HingeSteering::Translate( const idVec3 &translation ) { +void idAFConstraint_HingeSteering::Translate(const idVec3& translation) { } /* @@ -2255,7 +2258,7 @@ void idAFConstraint_HingeSteering::Translate( const idVec3 &translation ) { idAFConstraint_HingeSteering::Rotate ================ */ -void idAFConstraint_HingeSteering::Rotate( const idRotation &rotation ) { +void idAFConstraint_HingeSteering::Rotate(const idRotation& rotation) { } @@ -2270,18 +2273,18 @@ void idAFConstraint_HingeSteering::Rotate( const idRotation &rotation ) { idAFConstraint_Slider::idAFConstraint_Slider ================ */ -idAFConstraint_Slider::idAFConstraint_Slider( const idStr &name, idAFBody *body1, idAFBody *body2 ) { - assert( body1 ); +idAFConstraint_Slider::idAFConstraint_Slider(const idStr& name, idAFBody* body1, idAFBody* body2) { + assert(body1); type = CONSTRAINT_SLIDER; this->name = name; this->body1 = body1; this->body2 = body2; - InitSize( 5 ); + InitSize(5); fl.allowPrimary = true; fl.noCollision = true; - if ( body2 ) { - offset = ( body1->GetWorldOrigin() - body2->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose(); + if (body2) { + offset = (body1->GetWorldOrigin() - body2->GetWorldOrigin()) * body1->GetWorldAxis().Transpose(); relAxis = body1->GetWorldAxis() * body2->GetWorldAxis().Transpose(); } else { @@ -2295,13 +2298,13 @@ idAFConstraint_Slider::idAFConstraint_Slider( const idStr &name, idAFBody *body1 idAFConstraint_Slider::SetAxis ================ */ -void idAFConstraint_Slider::SetAxis( const idVec3 &ax ) { +void idAFConstraint_Slider::SetAxis(const idVec3& ax) { idVec3 normAxis; // get normalized axis relative to body1 normAxis = ax; normAxis.Normalize(); - if ( body2 ) { + if (body2) { axis = normAxis * body2->GetWorldAxis().Transpose(); } else { @@ -2314,44 +2317,44 @@ void idAFConstraint_Slider::SetAxis( const idVec3 &ax ) { idAFConstraint_Slider::Evaluate ================ */ -void idAFConstraint_Slider::Evaluate( float invTimeStep ) { +void idAFConstraint_Slider::Evaluate(float invTimeStep) { idVec3 vecX, vecY, ofs; idRotation r; - idAFBody *master; + idAFBody* master; master = body2 ? body2 : physics->GetMasterBody(); - if ( master ) { - (axis * master->GetWorldAxis()).OrthogonalBasis( vecX, vecY ); + if (master) { + (axis * master->GetWorldAxis()).OrthogonalBasis(vecX, vecY); ofs = master->GetWorldOrigin() + master->GetWorldAxis() * offset - body1->GetWorldOrigin(); - r = ( body1->GetWorldAxis().Transpose() * (relAxis * master->GetWorldAxis()) ).ToRotation(); + r = (body1->GetWorldAxis().Transpose() * (relAxis * master->GetWorldAxis())).ToRotation(); } else { - axis.OrthogonalBasis( vecX, vecY ); + axis.OrthogonalBasis(vecX, vecY); ofs = offset - body1->GetWorldOrigin(); - r = ( body1->GetWorldAxis().Transpose() * relAxis ).ToRotation(); + r = (body1->GetWorldAxis().Transpose() * relAxis).ToRotation(); } - J1.Set( mat3_zero, mat3_identity, - idMat3( vecX, vecY, vec3_origin ), mat3_zero ); - J1.SetSize( 5, 6 ); + J1.Set(mat3_zero, mat3_identity, + idMat3(vecX, vecY, vec3_origin), mat3_zero); + J1.SetSize(5, 6); - if ( body2 ) { + if (body2) { - J2.Set( mat3_zero, -mat3_identity, - idMat3( -vecX, -vecY, vec3_origin ), mat3_zero ); - J2.SetSize( 5, 6 ); + J2.Set(mat3_zero, -mat3_identity, + idMat3(-vecX, -vecY, vec3_origin), mat3_zero); + J2.SetSize(5, 6); } else { - J2.Zero( 5, 6 ); + J2.Zero(5, 6); } - c1.SubVec3(0) = -( invTimeStep * ERROR_REDUCTION ) * ( r.GetVec() * - (float) DEG2RAD( r.GetAngle() ) ); + c1.SubVec3(0) = -(invTimeStep * ERROR_REDUCTION) * (r.GetVec() * -(float)DEG2RAD(r.GetAngle())); - c1[3] = -( invTimeStep * ERROR_REDUCTION ) * ( vecX * ofs ); - c1[4] = -( invTimeStep * ERROR_REDUCTION ) * ( vecY * ofs ); + c1[3] = -(invTimeStep * ERROR_REDUCTION) * (vecX * ofs); + c1[4] = -(invTimeStep * ERROR_REDUCTION) * (vecY * ofs); - c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX ); + c1.Clamp(-ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX); } /* @@ -2359,7 +2362,7 @@ void idAFConstraint_Slider::Evaluate( float invTimeStep ) { idAFConstraint_Slider::ApplyFriction ================ */ -void idAFConstraint_Slider::ApplyFriction( float invTimeStep ) { +void idAFConstraint_Slider::ApplyFriction(float invTimeStep) { // no friction } @@ -2368,8 +2371,8 @@ void idAFConstraint_Slider::ApplyFriction( float invTimeStep ) { idAFConstraint_Slider::Translate ================ */ -void idAFConstraint_Slider::Translate( const idVec3 &translation ) { - if ( !body2 ) { +void idAFConstraint_Slider::Translate(const idVec3& translation) { + if (!body2) { offset += translation; } } @@ -2379,8 +2382,8 @@ void idAFConstraint_Slider::Translate( const idVec3 &translation ) { idAFConstraint_Slider::Rotate ================ */ -void idAFConstraint_Slider::Rotate( const idRotation &rotation ) { - if ( !body2 ) { +void idAFConstraint_Slider::Rotate(const idRotation& rotation) { + if (!body2) { offset *= rotation; } } @@ -2390,11 +2393,11 @@ void idAFConstraint_Slider::Rotate( const idRotation &rotation ) { idAFConstraint_Slider::GetCenter ================ */ -void idAFConstraint_Slider::GetCenter( idVec3 ¢er ) { - idAFBody *master; +void idAFConstraint_Slider::GetCenter(idVec3& center) { + idAFBody* master; master = body2 ? body2 : physics->GetMasterBody(); - if ( master ) { + if (master) { center = master->GetWorldOrigin() + master->GetWorldAxis() * offset - body1->GetWorldOrigin(); } else { @@ -2407,18 +2410,18 @@ void idAFConstraint_Slider::GetCenter( idVec3 ¢er ) { idAFConstraint_Slider::DebugDraw ================ */ -void idAFConstraint_Slider::DebugDraw( void ) { +void idAFConstraint_Slider::DebugDraw(void) { idVec3 ofs; - idAFBody *master; + idAFBody* master; master = body2 ? body2 : physics->GetMasterBody(); - if ( master ) { + if (master) { ofs = master->GetWorldOrigin() + master->GetWorldAxis() * offset - body1->GetWorldOrigin(); } else { ofs = offset - body1->GetWorldOrigin(); } - gameRenderWorld->DebugLine( colorGreen, ofs, ofs + axis * body1->GetWorldAxis() ); + gameRenderWorld->DebugLine(colorGreen, ofs, ofs + axis * body1->GetWorldAxis()); } /* @@ -2426,11 +2429,11 @@ void idAFConstraint_Slider::DebugDraw( void ) { idAFConstraint_Slider::Save ================ */ -void idAFConstraint_Slider::Save( idSaveGame *saveFile ) const { - idAFConstraint::Save( saveFile ); - saveFile->WriteVec3( axis ); - saveFile->WriteVec3( offset ); - saveFile->WriteMat3( relAxis ); +void idAFConstraint_Slider::Save(idSaveGame* saveFile) const { + idAFConstraint::Save(saveFile); + saveFile->WriteVec3(axis); + saveFile->WriteVec3(offset); + saveFile->WriteMat3(relAxis); } /* @@ -2438,11 +2441,11 @@ void idAFConstraint_Slider::Save( idSaveGame *saveFile ) const { idAFConstraint_Slider::Restore ================ */ -void idAFConstraint_Slider::Restore( idRestoreGame *saveFile ) { - idAFConstraint::Restore( saveFile ); - saveFile->ReadVec3( axis ); - saveFile->ReadVec3( offset ); - saveFile->ReadMat3( relAxis ); +void idAFConstraint_Slider::Restore(idRestoreGame* saveFile) { + idAFConstraint::Restore(saveFile); + saveFile->ReadVec3(axis); + saveFile->ReadVec3(offset); + saveFile->ReadMat3(relAxis); } @@ -2457,8 +2460,8 @@ void idAFConstraint_Slider::Restore( idRestoreGame *saveFile ) { idAFConstraint_Line::idAFConstraint_Line ================ */ -idAFConstraint_Line::idAFConstraint_Line( const idStr &name, idAFBody *body1, idAFBody *body2 ) { - assert( 0 ); // FIXME: implement +idAFConstraint_Line::idAFConstraint_Line(const idStr& name, idAFBody* body1, idAFBody* body2) { + assert(0); // FIXME: implement } /* @@ -2466,8 +2469,8 @@ idAFConstraint_Line::idAFConstraint_Line( const idStr &name, idAFBody *body1, id idAFConstraint_Line::Evaluate ================ */ -void idAFConstraint_Line::Evaluate( float invTimeStep ) { - assert( 0 ); // FIXME: implement +void idAFConstraint_Line::Evaluate(float invTimeStep) { + assert(0); // FIXME: implement } /* @@ -2475,8 +2478,8 @@ void idAFConstraint_Line::Evaluate( float invTimeStep ) { idAFConstraint_Line::ApplyFriction ================ */ -void idAFConstraint_Line::ApplyFriction( float invTimeStep ) { - assert( 0 ); // FIXME: implement +void idAFConstraint_Line::ApplyFriction(float invTimeStep) { + assert(0); // FIXME: implement } /* @@ -2484,8 +2487,8 @@ void idAFConstraint_Line::ApplyFriction( float invTimeStep ) { idAFConstraint_Line::Translate ================ */ -void idAFConstraint_Line::Translate( const idVec3 &translation ) { - assert( 0 ); // FIXME: implement +void idAFConstraint_Line::Translate(const idVec3& translation) { + assert(0); // FIXME: implement } /* @@ -2493,8 +2496,8 @@ void idAFConstraint_Line::Translate( const idVec3 &translation ) { idAFConstraint_Line::Rotate ================ */ -void idAFConstraint_Line::Rotate( const idRotation &rotation ) { - assert( 0 ); // FIXME: implement +void idAFConstraint_Line::Rotate(const idRotation& rotation) { + assert(0); // FIXME: implement } /* @@ -2502,8 +2505,8 @@ void idAFConstraint_Line::Rotate( const idRotation &rotation ) { idAFConstraint_Line::DebugDraw ================ */ -void idAFConstraint_Line::DebugDraw( void ) { - assert( 0 ); // FIXME: implement +void idAFConstraint_Line::DebugDraw(void) { + assert(0); // FIXME: implement } @@ -2518,13 +2521,13 @@ void idAFConstraint_Line::DebugDraw( void ) { idAFConstraint_Plane::idAFConstraint_Plane ================ */ -idAFConstraint_Plane::idAFConstraint_Plane( const idStr &name, idAFBody *body1, idAFBody *body2 ) { - assert( body1 ); +idAFConstraint_Plane::idAFConstraint_Plane(const idStr& name, idAFBody* body1, idAFBody* body2) { + assert(body1); type = CONSTRAINT_PLANE; this->name = name; this->body1 = body1; this->body2 = body2; - InitSize( 1 ); + InitSize(1); fl.allowPrimary = true; fl.noCollision = true; } @@ -2534,12 +2537,12 @@ idAFConstraint_Plane::idAFConstraint_Plane( const idStr &name, idAFBody *body1, idAFConstraint_Plane::SetPlane ================ */ -void idAFConstraint_Plane::SetPlane( const idVec3 &normal, const idVec3 &anchor ) { +void idAFConstraint_Plane::SetPlane(const idVec3& normal, const idVec3& anchor) { // get anchor relative to center of mass of body1 - anchor1 = ( anchor - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose(); - if ( body2 ) { + anchor1 = (anchor - body1->GetWorldOrigin()) * body1->GetWorldAxis().Transpose(); + if (body2) { // get anchor relative to center of mass of body2 - anchor2 = ( anchor - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose(); + anchor2 = (anchor - body2->GetWorldOrigin()) * body2->GetWorldAxis().Transpose(); planeNormal = normal * body2->GetWorldAxis().Transpose(); } else { @@ -2553,15 +2556,15 @@ void idAFConstraint_Plane::SetPlane( const idVec3 &normal, const idVec3 &anchor idAFConstraint_Plane::Evaluate ================ */ -void idAFConstraint_Plane::Evaluate( float invTimeStep ) { +void idAFConstraint_Plane::Evaluate(float invTimeStep) { idVec3 a1, a2, normal, p; idVec6 v; - idAFBody *master; + idAFBody* master; master = body2 ? body2 : physics->GetMasterBody(); a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); - if ( master ) { + if (master) { a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis(); normal = planeNormal * master->GetWorldAxis(); } @@ -2572,19 +2575,19 @@ void idAFConstraint_Plane::Evaluate( float invTimeStep ) { p = a1 - body1->GetWorldOrigin(); v.SubVec3(0) = normal; - v.SubVec3(1) = p.Cross( normal ); - J1.Set( 1, 6, v.ToFloatPtr() ); + v.SubVec3(1) = p.Cross(normal); + J1.Set(1, 6, v.ToFloatPtr()); - if ( body2 ) { + if (body2) { p = a1 - body2->GetWorldOrigin(); v.SubVec3(0) = -normal; - v.SubVec3(1) = p.Cross( -normal ); - J2.Set( 1, 6, v.ToFloatPtr() ); + v.SubVec3(1) = p.Cross(-normal); + J2.Set(1, 6, v.ToFloatPtr()); } - c1[0] = -( invTimeStep * ERROR_REDUCTION ) * (a1 * normal - a2 * normal); + c1[0] = -(invTimeStep * ERROR_REDUCTION) * (a1 * normal - a2 * normal); - c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX ); + c1.Clamp(-ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX); } /* @@ -2592,7 +2595,7 @@ void idAFConstraint_Plane::Evaluate( float invTimeStep ) { idAFConstraint_Plane::ApplyFriction ================ */ -void idAFConstraint_Plane::ApplyFriction( float invTimeStep ) { +void idAFConstraint_Plane::ApplyFriction(float invTimeStep) { // no friction } @@ -2601,8 +2604,8 @@ void idAFConstraint_Plane::ApplyFriction( float invTimeStep ) { idAFConstraint_Plane::Translate ================ */ -void idAFConstraint_Plane::Translate( const idVec3 &translation ) { - if ( !body2 ) { +void idAFConstraint_Plane::Translate(const idVec3& translation) { + if (!body2) { anchor2 += translation; } } @@ -2612,8 +2615,8 @@ void idAFConstraint_Plane::Translate( const idVec3 &translation ) { idAFConstraint_Plane::Rotate ================ */ -void idAFConstraint_Plane::Rotate( const idRotation &rotation ) { - if ( !body2 ) { +void idAFConstraint_Plane::Rotate(const idRotation& rotation) { + if (!body2) { anchor2 *= rotation; planeNormal *= rotation.ToMat3(); } @@ -2624,27 +2627,27 @@ void idAFConstraint_Plane::Rotate( const idRotation &rotation ) { idAFConstraint_Plane::DebugDraw ================ */ -void idAFConstraint_Plane::DebugDraw( void ) { +void idAFConstraint_Plane::DebugDraw(void) { idVec3 a1, normal, right, up; - idAFBody *master; + idAFBody* master; master = body2 ? body2 : physics->GetMasterBody(); a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); - if ( master ) { + if (master) { normal = planeNormal * master->GetWorldAxis(); } else { normal = planeNormal; } - normal.NormalVectors( right, up ); + normal.NormalVectors(right, up); normal *= 4.0f; right *= 4.0f; up *= 4.0f; - gameRenderWorld->DebugLine( colorCyan, a1 - right, a1 + right ); - gameRenderWorld->DebugLine( colorCyan, a1 - up, a1 + up ); - gameRenderWorld->DebugArrow( colorCyan, a1, a1 + normal, 1 ); + gameRenderWorld->DebugLine(colorCyan, a1 - right, a1 + right); + gameRenderWorld->DebugLine(colorCyan, a1 - up, a1 + up); + gameRenderWorld->DebugArrow(colorCyan, a1, a1 + normal, 1); } /* @@ -2652,11 +2655,11 @@ void idAFConstraint_Plane::DebugDraw( void ) { idAFConstraint_Plane::Save ================ */ -void idAFConstraint_Plane::Save( idSaveGame *saveFile ) const { - idAFConstraint::Save( saveFile ); - saveFile->WriteVec3( anchor1 ); - saveFile->WriteVec3( anchor2 ); - saveFile->WriteVec3( planeNormal ); +void idAFConstraint_Plane::Save(idSaveGame* saveFile) const { + idAFConstraint::Save(saveFile); + saveFile->WriteVec3(anchor1); + saveFile->WriteVec3(anchor2); + saveFile->WriteVec3(planeNormal); } /* @@ -2664,11 +2667,11 @@ void idAFConstraint_Plane::Save( idSaveGame *saveFile ) const { idAFConstraint_Plane::Restore ================ */ -void idAFConstraint_Plane::Restore( idRestoreGame *saveFile ) { - idAFConstraint::Restore( saveFile ); - saveFile->ReadVec3( anchor1 ); - saveFile->ReadVec3( anchor2 ); - saveFile->ReadVec3( planeNormal ); +void idAFConstraint_Plane::Restore(idRestoreGame* saveFile) { + idAFConstraint::Restore(saveFile); + saveFile->ReadVec3(anchor1); + saveFile->ReadVec3(anchor2); + saveFile->ReadVec3(planeNormal); } @@ -2683,13 +2686,13 @@ void idAFConstraint_Plane::Restore( idRestoreGame *saveFile ) { idAFConstraint_Spring::idAFConstraint_Spring ================ */ -idAFConstraint_Spring::idAFConstraint_Spring( const idStr &name, idAFBody *body1, idAFBody *body2 ) { - assert( body1 ); +idAFConstraint_Spring::idAFConstraint_Spring(const idStr& name, idAFBody* body1, idAFBody* body2) { + assert(body1); type = CONSTRAINT_SPRING; this->name = name; this->body1 = body1; this->body2 = body2; - InitSize( 1 ); + InitSize(1); fl.allowPrimary = false; kstretch = kcompress = damping = 1.0f; minLength = maxLength = restLength = 0.0f; @@ -2700,12 +2703,12 @@ idAFConstraint_Spring::idAFConstraint_Spring( const idStr &name, idAFBody *body1 idAFConstraint_Spring::SetAnchor ================ */ -void idAFConstraint_Spring::SetAnchor( const idVec3 &worldAnchor1, const idVec3 &worldAnchor2 ) { +void idAFConstraint_Spring::SetAnchor(const idVec3& worldAnchor1, const idVec3& worldAnchor2) { // get anchor relative to center of mass of body1 - anchor1 = ( worldAnchor1 - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose(); - if ( body2 ) { + anchor1 = (worldAnchor1 - body1->GetWorldOrigin()) * body1->GetWorldAxis().Transpose(); + if (body2) { // get anchor relative to center of mass of body2 - anchor2 = ( worldAnchor2 - body2->GetWorldOrigin() ) * body2->GetWorldAxis().Transpose(); + anchor2 = (worldAnchor2 - body2->GetWorldOrigin()) * body2->GetWorldAxis().Transpose(); } else { anchor2 = worldAnchor2; @@ -2717,8 +2720,8 @@ void idAFConstraint_Spring::SetAnchor( const idVec3 &worldAnchor1, const idVec3 idAFConstraint_Spring::SetSpring ================ */ -void idAFConstraint_Spring::SetSpring( const float stretch, const float compress, const float damping, const float restLength ) { - assert( stretch >= 0.0f && compress >= 0.0f && restLength >= 0.0f ); +void idAFConstraint_Spring::SetSpring(const float stretch, const float compress, const float damping, const float restLength) { + assert(stretch >= 0.0f && compress >= 0.0f && restLength >= 0.0f); this->kstretch = stretch; this->kcompress = compress; this->damping = damping; @@ -2730,8 +2733,8 @@ void idAFConstraint_Spring::SetSpring( const float stretch, const float compress idAFConstraint_Spring::SetLimit ================ */ -void idAFConstraint_Spring::SetLimit( const float minLength, const float maxLength ) { - assert( minLength >= 0.0f && maxLength >= 0.0f && maxLength >= minLength ); +void idAFConstraint_Spring::SetLimit(const float minLength, const float maxLength) { + assert(minLength >= 0.0f && maxLength >= 0.0f && (minLength == 0.0f || maxLength == 0.0f || maxLength >= minLength)); this->minLength = minLength; this->maxLength = maxLength; } @@ -2741,21 +2744,21 @@ void idAFConstraint_Spring::SetLimit( const float minLength, const float maxLeng idAFConstraint_Spring::Evaluate ================ */ -void idAFConstraint_Spring::Evaluate( float invTimeStep ) { +void idAFConstraint_Spring::Evaluate(float invTimeStep) { idVec3 a1, a2, velocity1, velocity2, force; idVec6 v1, v2; float d, dampingForce, length, error; bool limit; - idAFBody *master; + idAFBody* master; master = body2 ? body2 : physics->GetMasterBody(); a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); - velocity1 = body1->GetPointVelocity( a1 ); + velocity1 = body1->GetPointVelocity(a1); - if ( master ) { + if (master) { a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis(); - velocity2 = master->GetPointVelocity( a2 ); + velocity2 = master->GetPointVelocity(a2); } else { a2 = anchor2; @@ -2764,40 +2767,40 @@ void idAFConstraint_Spring::Evaluate( float invTimeStep ) { force = a2 - a1; d = force * force; - if ( d != 0.0f ) { - dampingForce = damping * idMath::Fabs( (velocity2 - velocity1) * force ) / d; + if (d != 0.0f) { + dampingForce = damping * idMath::Fabs((velocity2 - velocity1) * force) / d; } else { dampingForce = 0.0f; } length = force.Normalize(); - if ( length > restLength ) { - if ( kstretch > 0.0f ) { - idVec3 springForce = force * ( Square( length - restLength ) * kstretch - dampingForce ); - body1->AddForce( a1, springForce ); - if ( master ) { - master->AddForce( a2, -springForce ); + if (length > restLength) { + if (kstretch > 0.0f) { + idVec3 springForce = force * (Square(length - restLength) * kstretch - dampingForce); + body1->AddForce(a1, springForce); + if (master) { + master->AddForce(a2, -springForce); } } } else { - if ( kcompress > 0.0f ) { - idVec3 springForce = force * -( Square( restLength - length ) * kcompress - dampingForce ); - body1->AddForce( a1, springForce ); - if ( master ) { - master->AddForce( a2, -springForce ); + if (kcompress > 0.0f) { + idVec3 springForce = force * -(Square(restLength - length) * kcompress - dampingForce); + body1->AddForce(a1, springForce); + if (master) { + master->AddForce(a2, -springForce); } } } // check for spring limits - if ( length < minLength ) { + if (length < minLength) { force = -force; error = minLength - length; limit = true; } - else if ( maxLength > 0.0f && length > maxLength ) { + else if (maxLength > 0.0f && length > maxLength) { error = length - maxLength; limit = true; } @@ -2806,26 +2809,26 @@ void idAFConstraint_Spring::Evaluate( float invTimeStep ) { limit = false; } - if ( limit ) { + if (limit) { a1 -= body1->GetWorldOrigin(); v1.SubVec3(0) = force; - v1.SubVec3(1) = a1.Cross( force ); - J1.Set( 1, 6, v1.ToFloatPtr() ); - if ( body2 ) { + v1.SubVec3(1) = a1.Cross(force); + J1.Set(1, 6, v1.ToFloatPtr()); + if (body2) { a2 -= body2->GetWorldOrigin(); v2.SubVec3(0) = -force; - v2.SubVec3(1) = a2.Cross( -force ); - J2.Set( 1, 6, v2.ToFloatPtr() ); + v2.SubVec3(1) = a2.Cross(-force); + J2.Set(1, 6, v2.ToFloatPtr()); } - c1[0] = -( invTimeStep * ERROR_REDUCTION ) * error; + c1[0] = -(invTimeStep * ERROR_REDUCTION) * error; lo[0] = 0.0f; } else { - J1.Zero( 0, 0 ); - J2.Zero( 0, 0 ); + J1.Zero(0, 0); + J2.Zero(0, 0); } - c1.Clamp( -ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX ); + c1.Clamp(-ERROR_REDUCTION_MAX, ERROR_REDUCTION_MAX); } /* @@ -2833,7 +2836,7 @@ void idAFConstraint_Spring::Evaluate( float invTimeStep ) { idAFConstraint_Spring::ApplyFriction ================ */ -void idAFConstraint_Spring::ApplyFriction( float invTimeStep ) { +void idAFConstraint_Spring::ApplyFriction(float invTimeStep) { // no friction } @@ -2842,8 +2845,8 @@ void idAFConstraint_Spring::ApplyFriction( float invTimeStep ) { idAFConstraint_Spring::Translate ================ */ -void idAFConstraint_Spring::Translate( const idVec3 &translation ) { - if ( !body2 ) { +void idAFConstraint_Spring::Translate(const idVec3& translation) { + if (!body2) { anchor2 += translation; } } @@ -2853,8 +2856,8 @@ void idAFConstraint_Spring::Translate( const idVec3 &translation ) { idAFConstraint_Spring::Rotate ================ */ -void idAFConstraint_Spring::Rotate( const idRotation &rotation ) { - if ( !body2 ) { +void idAFConstraint_Spring::Rotate(const idRotation& rotation) { + if (!body2) { anchor2 *= rotation; } } @@ -2864,19 +2867,19 @@ void idAFConstraint_Spring::Rotate( const idRotation &rotation ) { idAFConstraint_Spring::GetCenter ================ */ -void idAFConstraint_Spring::GetCenter( idVec3 ¢er ) { - idAFBody *master; +void idAFConstraint_Spring::GetCenter(idVec3& center) { + idAFBody* master; idVec3 a1, a2; master = body2 ? body2 : physics->GetMasterBody(); a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); - if ( master ) { + if (master) { a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis(); } else { a2 = anchor2; } - center = ( a1 + a2 ) * 0.5f; + center = (a1 + a2) * 0.5f; } /* @@ -2884,14 +2887,14 @@ void idAFConstraint_Spring::GetCenter( idVec3 ¢er ) { idAFConstraint_Spring::DebugDraw ================ */ -void idAFConstraint_Spring::DebugDraw( void ) { - idAFBody *master; +void idAFConstraint_Spring::DebugDraw(void) { + idAFBody* master; float length; idVec3 a1, a2, dir, mid, p; master = body2 ? body2 : physics->GetMasterBody(); a1 = body1->GetWorldOrigin() + anchor1 * body1->GetWorldAxis(); - if ( master ) { + if (master) { a2 = master->GetWorldOrigin() + anchor2 * master->GetWorldAxis(); } else { @@ -2902,27 +2905,27 @@ void idAFConstraint_Spring::DebugDraw( void ) { length = dir.Normalize(); // draw spring - gameRenderWorld->DebugLine( colorGreen, a1, a2 ); + gameRenderWorld->DebugLine(colorGreen, a1, a2); // draw rest length p = restLength * 0.5f * dir; - gameRenderWorld->DebugCircle( colorWhite, mid + p, dir, 1.0f, 10 ); - gameRenderWorld->DebugCircle( colorWhite, mid - p, dir, 1.0f, 10 ); - if ( restLength > length ) { - gameRenderWorld->DebugLine( colorWhite, a2, mid + p ); - gameRenderWorld->DebugLine( colorWhite, a1, mid - p ); + gameRenderWorld->DebugCircle(colorWhite, mid + p, dir, 1.0f, 10); + gameRenderWorld->DebugCircle(colorWhite, mid - p, dir, 1.0f, 10); + if (restLength > length) { + gameRenderWorld->DebugLine(colorWhite, a2, mid + p); + gameRenderWorld->DebugLine(colorWhite, a1, mid - p); } - if ( minLength > 0.0f ) { + if (minLength > 0.0f) { // draw min length - gameRenderWorld->DebugCircle( colorBlue, mid + minLength * 0.5f * dir, dir, 2.0f, 10 ); - gameRenderWorld->DebugCircle( colorBlue, mid - minLength * 0.5f * dir, dir, 2.0f, 10 ); + gameRenderWorld->DebugCircle(colorBlue, mid + minLength * 0.5f * dir, dir, 2.0f, 10); + gameRenderWorld->DebugCircle(colorBlue, mid - minLength * 0.5f * dir, dir, 2.0f, 10); } - if ( maxLength > 0.0f ) { + if (maxLength > 0.0f) { // draw max length - gameRenderWorld->DebugCircle( colorRed, mid + maxLength * 0.5f * dir, dir, 2.0f, 10 ); - gameRenderWorld->DebugCircle( colorRed, mid - maxLength * 0.5f * dir, dir, 2.0f, 10 ); + gameRenderWorld->DebugCircle(colorRed, mid + maxLength * 0.5f * dir, dir, 2.0f, 10); + gameRenderWorld->DebugCircle(colorRed, mid - maxLength * 0.5f * dir, dir, 2.0f, 10); } } @@ -2931,16 +2934,16 @@ void idAFConstraint_Spring::DebugDraw( void ) { idAFConstraint_Spring::Save ================ */ -void idAFConstraint_Spring::Save( idSaveGame *saveFile ) const { - idAFConstraint::Save( saveFile ); - saveFile->WriteVec3( anchor1 ); - saveFile->WriteVec3( anchor2 ); - saveFile->WriteFloat( kstretch ); - saveFile->WriteFloat( kcompress ); - saveFile->WriteFloat( damping ); - saveFile->WriteFloat( restLength ); - saveFile->WriteFloat( minLength ); - saveFile->WriteFloat( maxLength ); +void idAFConstraint_Spring::Save(idSaveGame* saveFile) const { + idAFConstraint::Save(saveFile); + saveFile->WriteVec3(anchor1); + saveFile->WriteVec3(anchor2); + saveFile->WriteFloat(kstretch); + saveFile->WriteFloat(kcompress); + saveFile->WriteFloat(damping); + saveFile->WriteFloat(restLength); + saveFile->WriteFloat(minLength); + saveFile->WriteFloat(maxLength); } /* @@ -2948,16 +2951,16 @@ void idAFConstraint_Spring::Save( idSaveGame *saveFile ) const { idAFConstraint_Spring::Restore ================ */ -void idAFConstraint_Spring::Restore( idRestoreGame *saveFile ) { - idAFConstraint::Restore( saveFile ); - saveFile->ReadVec3( anchor1 ); - saveFile->ReadVec3( anchor2 ); - saveFile->ReadFloat( kstretch ); - saveFile->ReadFloat( kcompress ); - saveFile->ReadFloat( damping ); - saveFile->ReadFloat( restLength ); - saveFile->ReadFloat( minLength ); - saveFile->ReadFloat( maxLength ); +void idAFConstraint_Spring::Restore(idRestoreGame* saveFile) { + idAFConstraint::Restore(saveFile); + saveFile->ReadVec3(anchor1); + saveFile->ReadVec3(anchor2); + saveFile->ReadFloat(kstretch); + saveFile->ReadFloat(kcompress); + saveFile->ReadFloat(damping); + saveFile->ReadFloat(restLength); + saveFile->ReadFloat(minLength); + saveFile->ReadFloat(maxLength); } @@ -2972,10 +2975,10 @@ void idAFConstraint_Spring::Restore( idRestoreGame *saveFile ) { idAFConstraint_Contact::idAFConstraint_Contact ================ */ -idAFConstraint_Contact::idAFConstraint_Contact( void ) { +idAFConstraint_Contact::idAFConstraint_Contact(void) { name = "contact"; type = CONSTRAINT_CONTACT; - InitSize( 1 ); + InitSize(1); fc = NULL; fl.allowPrimary = false; fl.frameConstraint = true; @@ -2986,8 +2989,8 @@ idAFConstraint_Contact::idAFConstraint_Contact( void ) { idAFConstraint_Contact::~idAFConstraint_Contact ================ */ -idAFConstraint_Contact::~idAFConstraint_Contact( void ) { - if ( fc ) { +idAFConstraint_Contact::~idAFConstraint_Contact(void) { + if (fc) { delete fc; } } @@ -2997,13 +3000,13 @@ idAFConstraint_Contact::~idAFConstraint_Contact( void ) { idAFConstraint_Contact::Setup ================ */ -void idAFConstraint_Contact::Setup( idAFBody *b1, idAFBody *b2, contactInfo_t &c ) { +void idAFConstraint_Contact::Setup(idAFBody* b1, idAFBody* b2, contactInfo_t& c) { idVec3 p; idVec6 v; float vel; float minBounceVelocity = 2.0f; - assert( b1 ); + assert(b1); body1 = b1; body2 = b2; @@ -3011,22 +3014,23 @@ void idAFConstraint_Contact::Setup( idAFBody *b1, idAFBody *b2, contactInfo_t &c p = c.point - body1->GetWorldOrigin(); v.SubVec3(0) = c.normal; - v.SubVec3(1) = p.Cross( c.normal ); - J1.Set( 1, 6, v.ToFloatPtr() ); + v.SubVec3(1) = p.Cross(c.normal); + J1.Set(1, 6, v.ToFloatPtr()); vel = v.SubVec3(0) * body1->GetLinearVelocity() + v.SubVec3(1) * body1->GetAngularVelocity(); - if ( body2 ) { + if (body2) { p = c.point - body2->GetWorldOrigin(); v.SubVec3(0) = -c.normal; - v.SubVec3(1) = p.Cross( -c.normal ); - J2.Set( 1, 6, v.ToFloatPtr() ); + v.SubVec3(1) = p.Cross(-c.normal); + J2.Set(1, 6, v.ToFloatPtr()); vel += v.SubVec3(0) * body2->GetLinearVelocity() + v.SubVec3(1) * body2->GetAngularVelocity(); c2[0] = 0.0f; } - if ( body1->GetBouncyness() > 0.0f && -vel > minBounceVelocity ) { + if (body1->GetBouncyness() > 0.0f && -vel > minBounceVelocity) { c1[0] = body1->GetBouncyness() * vel; - } else { + } + else { c1[0] = 0.0f; } @@ -3042,7 +3046,7 @@ void idAFConstraint_Contact::Setup( idAFBody *b1, idAFBody *b2, contactInfo_t &c idAFConstraint_Contact::Evaluate ================ */ -void idAFConstraint_Contact::Evaluate( float invTimeStep ) { +void idAFConstraint_Contact::Evaluate(float invTimeStep) { // do nothing } @@ -3051,31 +3055,31 @@ void idAFConstraint_Contact::Evaluate( float invTimeStep ) { idAFConstraint_Contact::ApplyFriction ================ */ -void idAFConstraint_Contact::ApplyFriction( float invTimeStep ) { +void idAFConstraint_Contact::ApplyFriction(float invTimeStep) { idVec3 r, velocity, normal, dir1, dir2; float friction, magnitude, forceNumerator, forceDenominator; idVecX impulse, dv; friction = body1->GetContactFriction(); - if ( body2 && body2->GetContactFriction() < friction ) { + if (body2 && body2->GetContactFriction() < friction) { friction = body2->GetContactFriction(); } friction *= physics->GetContactFrictionScale(); - if ( friction <= 0.0f ) { + if (friction <= 0.0f) { return; } // seperate friction per contact is silly but it's fast and often looks close enough - if ( af_useImpulseFriction.GetBool() ) { + if (af_useImpulseFriction.GetBool()) { - impulse.SetData( 6, VECX_ALLOCA( 6 ) ); - dv.SetData( 6, VECX_ALLOCA( 6 ) ); + impulse.SetData(6, VECX_ALLOCA(6)); + dv.SetData(6, VECX_ALLOCA(6)); // calculate velocity in the contact plane r = contact.point - body1->GetWorldOrigin(); - velocity = body1->GetLinearVelocity() + body1->GetAngularVelocity().Cross( r ); + velocity = body1->GetLinearVelocity() + body1->GetAngularVelocity().Cross(r); velocity -= contact.normal * velocity * contact.normal; // get normalized direction of friction and magnitude of velocity @@ -3083,23 +3087,23 @@ void idAFConstraint_Contact::ApplyFriction( float invTimeStep ) { magnitude = normal.Normalize(); forceNumerator = friction * magnitude; - forceDenominator = body1->GetInverseMass() + ( ( body1->GetInverseWorldInertia() * r.Cross( normal ) ).Cross( r ) * normal ); + forceDenominator = body1->GetInverseMass() + ((body1->GetInverseWorldInertia() * r.Cross(normal)).Cross(r) * normal); impulse.SubVec3(0) = (forceNumerator / forceDenominator) * normal; - impulse.SubVec3(1) = r.Cross( impulse.SubVec3(0) ); - body1->InverseWorldSpatialInertiaMultiply( dv, impulse.ToFloatPtr() ); + impulse.SubVec3(1) = r.Cross(impulse.SubVec3(0)); + body1->InverseWorldSpatialInertiaMultiply(dv, impulse.ToFloatPtr()); // modify velocity with friction force - body1->SetLinearVelocity( body1->GetLinearVelocity() + dv.SubVec3(0) ); - body1->SetAngularVelocity( body1->GetAngularVelocity() + dv.SubVec3(1) ); + body1->SetLinearVelocity(body1->GetLinearVelocity() + dv.SubVec3(0)); + body1->SetAngularVelocity(body1->GetAngularVelocity() + dv.SubVec3(1)); } else { - if ( !fc ) { + if (!fc) { fc = new idAFConstraint_ContactFriction; } // call setup each frame because contact constraints are re-used for different bodies - fc->Setup( this ); - fc->Add( physics, invTimeStep ); + fc->Setup(this); + fc->Add(physics, invTimeStep); } } @@ -3108,8 +3112,8 @@ void idAFConstraint_Contact::ApplyFriction( float invTimeStep ) { idAFConstraint_Contact::Translate ================ */ -void idAFConstraint_Contact::Translate( const idVec3 &translation ) { - assert( 0 ); // contact should never be translated +void idAFConstraint_Contact::Translate(const idVec3& translation) { + assert(0); // contact should never be translated } /* @@ -3117,8 +3121,8 @@ void idAFConstraint_Contact::Translate( const idVec3 &translation ) { idAFConstraint_Contact::Rotate ================ */ -void idAFConstraint_Contact::Rotate( const idRotation &rotation ) { - assert( 0 ); // contact should never be rotated +void idAFConstraint_Contact::Rotate(const idRotation& rotation) { + assert(0); // contact should never be rotated } /* @@ -3126,7 +3130,7 @@ void idAFConstraint_Contact::Rotate( const idRotation &rotation ) { idAFConstraint_Contact::GetCenter ================ */ -void idAFConstraint_Contact::GetCenter( idVec3 ¢er ) { +void idAFConstraint_Contact::GetCenter(idVec3& center) { center = contact.point; } @@ -3135,12 +3139,12 @@ void idAFConstraint_Contact::GetCenter( idVec3 ¢er ) { idAFConstraint_Contact::DebugDraw ================ */ -void idAFConstraint_Contact::DebugDraw( void ) { +void idAFConstraint_Contact::DebugDraw(void) { idVec3 x, y; - contact.normal.NormalVectors( x, y ); - gameRenderWorld->DebugLine( colorWhite, contact.point, contact.point + 6.0f * contact.normal ); - gameRenderWorld->DebugLine( colorWhite, contact.point - 2.0f * x, contact.point + 2.0f * x ); - gameRenderWorld->DebugLine( colorWhite, contact.point - 2.0f * y, contact.point + 2.0f * y ); + contact.normal.NormalVectors(x, y); + gameRenderWorld->DebugLine(colorWhite, contact.point, contact.point + 6.0f * contact.normal); + gameRenderWorld->DebugLine(colorWhite, contact.point - 2.0f * x, contact.point + 2.0f * x); + gameRenderWorld->DebugLine(colorWhite, contact.point - 2.0f * y, contact.point + 2.0f * y); } @@ -3155,10 +3159,10 @@ void idAFConstraint_Contact::DebugDraw( void ) { idAFConstraint_ContactFriction::idAFConstraint_ContactFriction ================ */ -idAFConstraint_ContactFriction::idAFConstraint_ContactFriction( void ) { +idAFConstraint_ContactFriction::idAFConstraint_ContactFriction(void) { type = CONSTRAINT_FRICTION; name = "contactFriction"; - InitSize( 2 ); + InitSize(2); cc = NULL; fl.allowPrimary = false; fl.frameConstraint = true; @@ -3169,7 +3173,7 @@ idAFConstraint_ContactFriction::idAFConstraint_ContactFriction( void ) { idAFConstraint_ContactFriction::Setup ================ */ -void idAFConstraint_ContactFriction::Setup( idAFConstraint_Contact *cc ) { +void idAFConstraint_ContactFriction::Setup(idAFConstraint_Contact* cc) { this->cc = cc; body1 = cc->GetBody1(); body2 = cc->GetBody2(); @@ -3180,7 +3184,7 @@ void idAFConstraint_ContactFriction::Setup( idAFConstraint_Contact *cc ) { idAFConstraint_ContactFriction::Evaluate ================ */ -void idAFConstraint_ContactFriction::Evaluate( float invTimeStep ) { +void idAFConstraint_ContactFriction::Evaluate(float invTimeStep) { // do nothing } @@ -3189,7 +3193,7 @@ void idAFConstraint_ContactFriction::Evaluate( float invTimeStep ) { idAFConstraint_ContactFriction::ApplyFriction ================ */ -void idAFConstraint_ContactFriction::ApplyFriction( float invTimeStep ) { +void idAFConstraint_ContactFriction::ApplyFriction(float invTimeStep) { // do nothing } @@ -3198,7 +3202,7 @@ void idAFConstraint_ContactFriction::ApplyFriction( float invTimeStep ) { idAFConstraint_ContactFriction::Add ================ */ -bool idAFConstraint_ContactFriction::Add( idPhysics_AF *phys, float invTimeStep ) { +bool idAFConstraint_ContactFriction::Add(idPhysics_AF* phys, float invTimeStep) { idVec3 r, dir1, dir2; float friction; int newRow; @@ -3208,26 +3212,26 @@ bool idAFConstraint_ContactFriction::Add( idPhysics_AF *phys, float invTimeStep friction = body1->GetContactFriction() * physics->GetContactFrictionScale(); // if the body only has friction in one direction - if ( body1->GetFrictionDirection( dir1 ) ) { + if (body1->GetFrictionDirection(dir1)) { // project the friction direction into the contact plane dir1 -= dir1 * cc->GetContact().normal * dir1; dir1.Normalize(); r = cc->GetContact().point - body1->GetWorldOrigin(); - J1.SetSize( 1, 6 ); + J1.SetSize(1, 6); J1.SubVec6(0).SubVec3(0) = dir1; - J1.SubVec6(0).SubVec3(1) = r.Cross( dir1 ); - c1.SetSize( 1 ); + J1.SubVec6(0).SubVec3(1) = r.Cross(dir1); + c1.SetSize(1); c1[0] = 0.0f; - if ( body2 ) { + if (body2) { r = cc->GetContact().point - body2->GetWorldOrigin(); - J2.SetSize( 1, 6 ); + J2.SetSize(1, 6); J2.SubVec6(0).SubVec3(0) = -dir1; - J2.SubVec6(0).SubVec3(1) = r.Cross( -dir1 ); - c2.SetSize( 1 ); + J2.SubVec6(0).SubVec3(1) = r.Cross(-dir1); + c2.SetSize(1); c2[0] = 0.0f; } @@ -3238,30 +3242,30 @@ bool idAFConstraint_ContactFriction::Add( idPhysics_AF *phys, float invTimeStep } else { // get two friction directions orthogonal to contact normal - cc->GetContact().normal.NormalVectors( dir1, dir2 ); + cc->GetContact().normal.NormalVectors(dir1, dir2); r = cc->GetContact().point - body1->GetWorldOrigin(); - J1.SetSize( 2, 6 ); + J1.SetSize(2, 6); J1.SubVec6(0).SubVec3(0) = dir1; - J1.SubVec6(0).SubVec3(1) = r.Cross( dir1 ); + J1.SubVec6(0).SubVec3(1) = r.Cross(dir1); J1.SubVec6(1).SubVec3(0) = dir2; - J1.SubVec6(1).SubVec3(1) = r.Cross( dir2 ); - c1.SetSize( 2 ); + J1.SubVec6(1).SubVec3(1) = r.Cross(dir2); + c1.SetSize(2); c1[0] = c1[1] = 0.0f; - if ( body2 ) { + if (body2) { r = cc->GetContact().point - body2->GetWorldOrigin(); - J2.SetSize( 2, 6 ); + J2.SetSize(2, 6); J2.SubVec6(0).SubVec3(0) = -dir1; - J2.SubVec6(0).SubVec3(1) = r.Cross( -dir1 ); + J2.SubVec6(0).SubVec3(1) = r.Cross(-dir1); J2.SubVec6(1).SubVec3(0) = -dir2; - J2.SubVec6(1).SubVec3(1) = r.Cross( -dir2 ); - c2.SetSize( 2 ); + J2.SubVec6(1).SubVec3(1) = r.Cross(-dir2); + c2.SetSize(2); c2[0] = c2[1] = 0.0f; - if ( body2->GetContactFriction() < friction ) { + if (body2->GetContactFriction() < friction) { friction = body2->GetContactFriction(); } } @@ -3275,7 +3279,7 @@ bool idAFConstraint_ContactFriction::Add( idPhysics_AF *phys, float invTimeStep boxIndex[1] = 0; } - if ( body1->GetContactMotorDirection( dir1 ) && body1->GetContactMotorForce() > 0.0f ) { + if (body1->GetContactMotorDirection(dir1) && body1->GetContactMotorForce() > 0.0f) { // project the motor force direction into the contact plane dir1 -= dir1 * cc->GetContact().normal * dir1; dir1.Normalize(); @@ -3283,19 +3287,19 @@ bool idAFConstraint_ContactFriction::Add( idPhysics_AF *phys, float invTimeStep r = cc->GetContact().point - body1->GetWorldOrigin(); newRow = J1.GetNumRows(); - J1.ChangeSize( newRow+1, J1.GetNumColumns() ); + J1.ChangeSize(newRow + 1, J1.GetNumColumns()); J1.SubVec6(newRow).SubVec3(0) = -dir1; - J1.SubVec6(newRow).SubVec3(1) = r.Cross( -dir1 ); - c1.ChangeSize( newRow+1 ); + J1.SubVec6(newRow).SubVec3(1) = r.Cross(-dir1); + c1.ChangeSize(newRow + 1); c1[newRow] = body1->GetContactMotorVelocity(); - if ( body2 ) { + if (body2) { r = cc->GetContact().point - body2->GetWorldOrigin(); - J2.ChangeSize( newRow+1, J2.GetNumColumns() ); + J2.ChangeSize(newRow + 1, J2.GetNumColumns()); J2.SubVec6(newRow).SubVec3(0) = -dir1; - J2.SubVec6(newRow).SubVec3(1) = r.Cross( -dir1 ); - c2.ChangeSize( newRow+1 ); + J2.SubVec6(newRow).SubVec3(1) = r.Cross(-dir1); + c2.ChangeSize(newRow + 1); c2[newRow] = 0.0f; } @@ -3304,7 +3308,7 @@ bool idAFConstraint_ContactFriction::Add( idPhysics_AF *phys, float invTimeStep boxIndex[newRow] = -1; } - physics->AddFrameConstraint( this ); + physics->AddFrameConstraint(this); return true; } @@ -3314,7 +3318,7 @@ bool idAFConstraint_ContactFriction::Add( idPhysics_AF *phys, float invTimeStep idAFConstraint_ContactFriction::Translate ================ */ -void idAFConstraint_ContactFriction::Translate( const idVec3 &translation ) { +void idAFConstraint_ContactFriction::Translate(const idVec3& translation) { } /* @@ -3322,7 +3326,7 @@ void idAFConstraint_ContactFriction::Translate( const idVec3 &translation ) { idAFConstraint_ContactFriction::Rotate ================ */ -void idAFConstraint_ContactFriction::Rotate( const idRotation &rotation ) { +void idAFConstraint_ContactFriction::Rotate(const idRotation& rotation) { } /* @@ -3330,7 +3334,7 @@ void idAFConstraint_ContactFriction::Rotate( const idRotation &rotation ) { idAFConstraint_ContactFriction::DebugDraw ================ */ -void idAFConstraint_ContactFriction::DebugDraw( void ) { +void idAFConstraint_ContactFriction::DebugDraw(void) { } @@ -3345,10 +3349,10 @@ void idAFConstraint_ContactFriction::DebugDraw( void ) { idAFConstraint_ConeLimit::idAFConstraint_ConeLimit ================ */ -idAFConstraint_ConeLimit::idAFConstraint_ConeLimit( void ) { +idAFConstraint_ConeLimit::idAFConstraint_ConeLimit(void) { type = CONSTRAINT_CONELIMIT; name = "coneLimit"; - InitSize( 1 ); + InitSize(1); fl.allowPrimary = false; fl.frameConstraint = true; } @@ -3363,7 +3367,7 @@ idAFConstraint_ConeLimit::Setup the body1Axis is the axis in body1 space that should stay within the cone ================ */ -void idAFConstraint_ConeLimit::Setup( idAFBody *b1, idAFBody *b2, const idVec3 &coneAnchor, const idVec3 &coneAxis, const float coneAngle, const idVec3 &body1Axis ) { +void idAFConstraint_ConeLimit::Setup(idAFBody* b1, idAFBody* b2, const idVec3& coneAnchor, const idVec3& coneAxis, const float coneAngle, const idVec3& body1Axis) { this->body1 = b1; this->body2 = b2; this->coneAxis = coneAxis; @@ -3371,9 +3375,9 @@ void idAFConstraint_ConeLimit::Setup( idAFBody *b1, idAFBody *b2, const idVec3 & this->coneAnchor = coneAnchor; this->body1Axis = body1Axis; this->body1Axis.Normalize(); - this->cosAngle = (float) cos( DEG2RAD( coneAngle * 0.5f ) ); - this->sinHalfAngle = (float) sin( DEG2RAD( coneAngle * 0.25f ) ); - this->cosHalfAngle = (float) cos( DEG2RAD( coneAngle * 0.25f ) ); + this->cosAngle = (float)cos(DEG2RAD(coneAngle * 0.5f)); + this->sinHalfAngle = (float)sin(DEG2RAD(coneAngle * 0.25f)); + this->cosHalfAngle = (float)cos(DEG2RAD(coneAngle * 0.25f)); } /* @@ -3381,7 +3385,7 @@ void idAFConstraint_ConeLimit::Setup( idAFBody *b1, idAFBody *b2, const idVec3 & idAFConstraint_ConeLimit::SetAnchor ================ */ -void idAFConstraint_ConeLimit::SetAnchor( const idVec3 &coneAnchor ) { +void idAFConstraint_ConeLimit::SetAnchor(const idVec3& coneAnchor) { this->coneAnchor = coneAnchor; } @@ -3390,7 +3394,7 @@ void idAFConstraint_ConeLimit::SetAnchor( const idVec3 &coneAnchor ) { idAFConstraint_ConeLimit::SetBody1Axis ================ */ -void idAFConstraint_ConeLimit::SetBody1Axis( const idVec3 &body1Axis ) { +void idAFConstraint_ConeLimit::SetBody1Axis(const idVec3& body1Axis) { this->body1Axis = body1Axis; } @@ -3399,7 +3403,7 @@ void idAFConstraint_ConeLimit::SetBody1Axis( const idVec3 &body1Axis ) { idAFConstraint_ConeLimit::Evaluate ================ */ -void idAFConstraint_ConeLimit::Evaluate( float invTimeStep ) { +void idAFConstraint_ConeLimit::Evaluate(float invTimeStep) { // do nothing } @@ -3408,7 +3412,7 @@ void idAFConstraint_ConeLimit::Evaluate( float invTimeStep ) { idAFConstraint_ConeLimit::ApplyFriction ================ */ -void idAFConstraint_ConeLimit::ApplyFriction( float invTimeStep ) { +void idAFConstraint_ConeLimit::ApplyFriction(float invTimeStep) { } /* @@ -3416,14 +3420,14 @@ void idAFConstraint_ConeLimit::ApplyFriction( float invTimeStep ) { idAFConstraint_ConeLimit::Add ================ */ -bool idAFConstraint_ConeLimit::Add( idPhysics_AF *phys, float invTimeStep ) { +bool idAFConstraint_ConeLimit::Add(idPhysics_AF* phys, float invTimeStep) { float a; idVec6 J1row, J2row; idVec3 ax, anchor, body1ax, normal, coneVector, p1, p2; idQuat q; - idAFBody *master; + idAFBody* master; - if ( af_skipLimits.GetBool() ) { + if (af_skipLimits.GetBool()) { lm.Zero(); // constraint exerts no force return false; } @@ -3432,7 +3436,7 @@ bool idAFConstraint_ConeLimit::Add( idPhysics_AF *phys, float invTimeStep ) { master = body2 ? body2 : physics->GetMasterBody(); - if ( master ) { + if (master) { ax = coneAxis * master->GetWorldAxis(); anchor = master->GetWorldOrigin() + coneAnchor * master->GetWorldAxis(); } @@ -3446,37 +3450,37 @@ bool idAFConstraint_ConeLimit::Add( idPhysics_AF *phys, float invTimeStep ) { a = ax * body1ax; // if the body1 axis is inside the cone - if ( a > cosAngle ) { + if (a > cosAngle) { lm.Zero(); // constraint exerts no force return false; } // calculate the inward cone normal for the position the body1 axis went outside the cone - normal = body1ax.Cross( ax ); + normal = body1ax.Cross(ax); normal.Normalize(); q.x = normal.x * sinHalfAngle; q.y = normal.y * sinHalfAngle; q.z = normal.z * sinHalfAngle; q.w = cosHalfAngle; coneVector = ax * q.ToMat3(); - normal = coneVector.Cross( ax ).Cross( coneVector ); + normal = coneVector.Cross(ax).Cross(coneVector); normal.Normalize(); p1 = anchor + 32.0f * coneVector - body1->GetWorldOrigin(); J1row.SubVec3(0) = normal; - J1row.SubVec3(1) = p1.Cross( normal ); - J1.Set( 1, 6, J1row.ToFloatPtr() ); + J1row.SubVec3(1) = p1.Cross(normal); + J1.Set(1, 6, J1row.ToFloatPtr()); - c1[0] = (invTimeStep * LIMIT_ERROR_REDUCTION) * ( normal * (32.0f * body1ax) ); + c1[0] = (invTimeStep * LIMIT_ERROR_REDUCTION) * (normal * (32.0f * body1ax)); - if ( body2 ) { + if (body2) { p2 = anchor + 32.0f * coneVector - master->GetWorldOrigin(); J2row.SubVec3(0) = -normal; - J2row.SubVec3(1) = p2.Cross( -normal ); - J2.Set( 1, 6, J2row.ToFloatPtr() ); + J2row.SubVec3(1) = p2.Cross(-normal); + J2.Set(1, 6, J2row.ToFloatPtr()); c2[0] = 0.0f; } @@ -3484,7 +3488,7 @@ bool idAFConstraint_ConeLimit::Add( idPhysics_AF *phys, float invTimeStep ) { lo[0] = 0.0f; e[0] = LIMIT_LCP_EPSILON; - physics->AddFrameConstraint( this ); + physics->AddFrameConstraint(this); return true; } @@ -3494,8 +3498,8 @@ bool idAFConstraint_ConeLimit::Add( idPhysics_AF *phys, float invTimeStep ) { idAFConstraint_ConeLimit::Translate ================ */ -void idAFConstraint_ConeLimit::Translate( const idVec3 &translation ) { - if ( !body2 ) { +void idAFConstraint_ConeLimit::Translate(const idVec3& translation) { + if (!body2) { coneAnchor += translation; } } @@ -3505,8 +3509,8 @@ void idAFConstraint_ConeLimit::Translate( const idVec3 &translation ) { idAFConstraint_ConeLimit::Rotate ================ */ -void idAFConstraint_ConeLimit::Rotate( const idRotation &rotation ) { - if ( !body2 ) { +void idAFConstraint_ConeLimit::Rotate(const idRotation& rotation) { + if (!body2) { coneAnchor *= rotation; coneAxis *= rotation.ToMat3(); } @@ -3517,14 +3521,14 @@ void idAFConstraint_ConeLimit::Rotate( const idRotation &rotation ) { idAFConstraint_ConeLimit::DebugDraw ================ */ -void idAFConstraint_ConeLimit::DebugDraw( void ) { +void idAFConstraint_ConeLimit::DebugDraw(void) { idVec3 ax, anchor, x, y, z, start, end; float sinAngle, a, size = 10.0f; - idAFBody *master; + idAFBody* master; master = body2 ? body2 : physics->GetMasterBody(); - if ( master ) { + if (master) { ax = coneAxis * master->GetWorldAxis(); anchor = master->GetWorldOrigin() + coneAnchor * master->GetWorldAxis(); } @@ -3534,19 +3538,19 @@ void idAFConstraint_ConeLimit::DebugDraw( void ) { } // draw body1 axis - gameRenderWorld->DebugLine( colorGreen, anchor, anchor + size * (body1Axis * body1->GetWorldAxis()) ); + gameRenderWorld->DebugLine(colorGreen, anchor, anchor + size * (body1Axis * body1->GetWorldAxis())); // draw cone - ax.NormalVectors( x, y ); - sinAngle = idMath::Sqrt( 1.0f - cosAngle * cosAngle ); + ax.NormalVectors(x, y); + sinAngle = idMath::Sqrt(1.0f - cosAngle * cosAngle); x *= size * sinAngle; y *= size * sinAngle; z = anchor + ax * size * cosAngle; start = x + z; - for ( a = 0.0f; a < 360.0f; a += 45.0f ) { - end = x * (float) cos( DEG2RAD(a + 45.0f) ) + y * (float) sin( DEG2RAD(a + 45.0f) ) + z; - gameRenderWorld->DebugLine( colorMagenta, anchor, start ); - gameRenderWorld->DebugLine( colorMagenta, start, end ); + for (a = 0.0f; a < 360.0f; a += 45.0f) { + end = x * (float)cos(DEG2RAD(a + 45.0f)) + y * (float)sin(DEG2RAD(a + 45.0f)) + z; + gameRenderWorld->DebugLine(colorMagenta, anchor, start); + gameRenderWorld->DebugLine(colorMagenta, start, end); start = end; } } @@ -3556,15 +3560,15 @@ void idAFConstraint_ConeLimit::DebugDraw( void ) { idAFConstraint_ConeLimit::Save ================ */ -void idAFConstraint_ConeLimit::Save( idSaveGame *saveFile ) const { - idAFConstraint::Save( saveFile ); - saveFile->WriteVec3( coneAnchor ); - saveFile->WriteVec3( coneAxis ); - saveFile->WriteVec3( body1Axis ); - saveFile->WriteFloat( cosAngle ); - saveFile->WriteFloat( sinHalfAngle ); - saveFile->WriteFloat( cosHalfAngle ); - saveFile->WriteFloat( epsilon ); +void idAFConstraint_ConeLimit::Save(idSaveGame* saveFile) const { + idAFConstraint::Save(saveFile); + saveFile->WriteVec3(coneAnchor); + saveFile->WriteVec3(coneAxis); + saveFile->WriteVec3(body1Axis); + saveFile->WriteFloat(cosAngle); + saveFile->WriteFloat(sinHalfAngle); + saveFile->WriteFloat(cosHalfAngle); + saveFile->WriteFloat(epsilon); } /* @@ -3572,15 +3576,15 @@ void idAFConstraint_ConeLimit::Save( idSaveGame *saveFile ) const { idAFConstraint_ConeLimit::Restore ================ */ -void idAFConstraint_ConeLimit::Restore( idRestoreGame *saveFile ) { - idAFConstraint::Restore( saveFile ); - saveFile->ReadVec3( coneAnchor ); - saveFile->ReadVec3( coneAxis ); - saveFile->ReadVec3( body1Axis ); - saveFile->ReadFloat( cosAngle ); - saveFile->ReadFloat( sinHalfAngle ); - saveFile->ReadFloat( cosHalfAngle ); - saveFile->ReadFloat( epsilon ); +void idAFConstraint_ConeLimit::Restore(idRestoreGame* saveFile) { + idAFConstraint::Restore(saveFile); + saveFile->ReadVec3(coneAnchor); + saveFile->ReadVec3(coneAxis); + saveFile->ReadVec3(body1Axis); + saveFile->ReadFloat(cosAngle); + saveFile->ReadFloat(sinHalfAngle); + saveFile->ReadFloat(cosHalfAngle); + saveFile->ReadFloat(epsilon); } @@ -3595,10 +3599,10 @@ void idAFConstraint_ConeLimit::Restore( idRestoreGame *saveFile ) { idAFConstraint_PyramidLimit::idAFConstraint_PyramidLimit ================ */ -idAFConstraint_PyramidLimit::idAFConstraint_PyramidLimit( void ) { +idAFConstraint_PyramidLimit::idAFConstraint_PyramidLimit(void) { type = CONSTRAINT_PYRAMIDLIMIT; name = "pyramidLimit"; - InitSize( 1 ); + InitSize(1); fl.allowPrimary = false; fl.frameConstraint = true; } @@ -3608,9 +3612,9 @@ idAFConstraint_PyramidLimit::idAFConstraint_PyramidLimit( void ) { idAFConstraint_PyramidLimit::Setup ================ */ -void idAFConstraint_PyramidLimit::Setup( idAFBody *b1, idAFBody *b2, const idVec3 &pyramidAnchor, - const idVec3 &pyramidAxis, const idVec3 &baseAxis, - const float pyramidAngle1, const float pyramidAngle2, const idVec3 &body1Axis ) { +void idAFConstraint_PyramidLimit::Setup(idAFBody* b1, idAFBody* b2, const idVec3& pyramidAnchor, + const idVec3& pyramidAxis, const idVec3& baseAxis, + const float pyramidAngle1, const float pyramidAngle2, const idVec3& body1Axis) { body1 = b1; body2 = b2; // setup the base and make sure the basis is orthonormal @@ -3619,16 +3623,16 @@ void idAFConstraint_PyramidLimit::Setup( idAFBody *b1, idAFBody *b2, const idVec pyramidBasis[0] = baseAxis; pyramidBasis[0] -= pyramidBasis[2] * baseAxis * pyramidBasis[2]; pyramidBasis[0].Normalize(); - pyramidBasis[1] = pyramidBasis[0].Cross( pyramidBasis[2] ); + pyramidBasis[1] = pyramidBasis[0].Cross(pyramidBasis[2]); // pyramid top this->pyramidAnchor = pyramidAnchor; // angles - cosAngle[0] = (float) cos( DEG2RAD( pyramidAngle1 * 0.5f ) ); - cosAngle[1] = (float) cos( DEG2RAD( pyramidAngle2 * 0.5f ) ); - sinHalfAngle[0] = (float) sin( DEG2RAD( pyramidAngle1 * 0.25f ) ); - sinHalfAngle[1] = (float) sin( DEG2RAD( pyramidAngle2 * 0.25f ) ); - cosHalfAngle[0] = (float) cos( DEG2RAD( pyramidAngle1 * 0.25f ) ); - cosHalfAngle[1] = (float) cos( DEG2RAD( pyramidAngle2 * 0.25f ) ); + cosAngle[0] = (float)cos(DEG2RAD(pyramidAngle1 * 0.5f)); + cosAngle[1] = (float)cos(DEG2RAD(pyramidAngle2 * 0.5f)); + sinHalfAngle[0] = (float)sin(DEG2RAD(pyramidAngle1 * 0.25f)); + sinHalfAngle[1] = (float)sin(DEG2RAD(pyramidAngle2 * 0.25f)); + cosHalfAngle[0] = (float)cos(DEG2RAD(pyramidAngle1 * 0.25f)); + cosHalfAngle[1] = (float)cos(DEG2RAD(pyramidAngle2 * 0.25f)); this->body1Axis = body1Axis; } @@ -3638,7 +3642,7 @@ void idAFConstraint_PyramidLimit::Setup( idAFBody *b1, idAFBody *b2, const idVec idAFConstraint_PyramidLimit::SetAnchor ================ */ -void idAFConstraint_PyramidLimit::SetAnchor( const idVec3 &pyramidAnchor ) { +void idAFConstraint_PyramidLimit::SetAnchor(const idVec3& pyramidAnchor) { this->pyramidAnchor = pyramidAnchor; } @@ -3647,7 +3651,7 @@ void idAFConstraint_PyramidLimit::SetAnchor( const idVec3 &pyramidAnchor ) { idAFConstraint_PyramidLimit::SetBody1Axis ================ */ -void idAFConstraint_PyramidLimit::SetBody1Axis( const idVec3 &body1Axis ) { +void idAFConstraint_PyramidLimit::SetBody1Axis(const idVec3& body1Axis) { this->body1Axis = body1Axis; } @@ -3656,7 +3660,7 @@ void idAFConstraint_PyramidLimit::SetBody1Axis( const idVec3 &body1Axis ) { idAFConstraint_PyramidLimit::Evaluate ================ */ -void idAFConstraint_PyramidLimit::Evaluate( float invTimeStep ) { +void idAFConstraint_PyramidLimit::Evaluate(float invTimeStep) { // do nothing } @@ -3665,7 +3669,7 @@ void idAFConstraint_PyramidLimit::Evaluate( float invTimeStep ) { idAFConstraint_PyramidLimit::ApplyFriction ================ */ -void idAFConstraint_PyramidLimit::ApplyFriction( float invTimeStep ) { +void idAFConstraint_PyramidLimit::ApplyFriction(float invTimeStep) { } /* @@ -3673,16 +3677,16 @@ void idAFConstraint_PyramidLimit::ApplyFriction( float invTimeStep ) { idAFConstraint_PyramidLimit::Add ================ */ -bool idAFConstraint_PyramidLimit::Add( idPhysics_AF *phys, float invTimeStep ) { +bool idAFConstraint_PyramidLimit::Add(idPhysics_AF* phys, float invTimeStep) { int i; float a[2]; idVec6 J1row, J2row; idMat3 worldBase; idVec3 anchor, body1ax, ax[2], v, normal, pyramidVector, p1, p2; idQuat q; - idAFBody *master; + idAFBody* master; - if ( af_skipLimits.GetBool() ) { + if (af_skipLimits.GetBool()) { lm.Zero(); // constraint exerts no force return false; } @@ -3690,7 +3694,7 @@ bool idAFConstraint_PyramidLimit::Add( idPhysics_AF *phys, float invTimeStep ) { physics = phys; master = body2 ? body2 : physics->GetMasterBody(); - if ( master ) { + if (master) { worldBase[0] = pyramidBasis[0] * master->GetWorldAxis(); worldBase[1] = pyramidBasis[1] * master->GetWorldAxis(); worldBase[2] = pyramidBasis[2] * master->GetWorldAxis(); @@ -3703,23 +3707,23 @@ bool idAFConstraint_PyramidLimit::Add( idPhysics_AF *phys, float invTimeStep ) { body1ax = body1Axis * body1->GetWorldAxis(); - for ( i = 0; i < 2; i++ ) { + for (i = 0; i < 2; i++) { ax[i] = body1ax - worldBase[!i] * body1ax * worldBase[!i]; ax[i].Normalize(); a[i] = worldBase[2] * ax[i]; } // if the body1 axis is inside the pyramid - if ( a[0] > cosAngle[0] && a[1] > cosAngle[1] ) { + if (a[0] > cosAngle[0] && a[1] > cosAngle[1]) { lm.Zero(); // constraint exerts no force return false; } // calculate the inward pyramid normal for the position the body1 axis went outside the pyramid pyramidVector = worldBase[2]; - for ( i = 0; i < 2; i++ ) { - if ( a[i] <= cosAngle[i] ) { - v = ax[i].Cross( worldBase[2] ); + for (i = 0; i < 2; i++) { + if (a[i] <= cosAngle[i]) { + v = ax[i].Cross(worldBase[2]); v.Normalize(); q.x = v.x * sinHalfAngle[i]; q.y = v.y * sinHalfAngle[i]; @@ -3728,24 +3732,24 @@ bool idAFConstraint_PyramidLimit::Add( idPhysics_AF *phys, float invTimeStep ) { pyramidVector *= q.ToMat3(); } } - normal = pyramidVector.Cross( worldBase[2] ).Cross( pyramidVector ); + normal = pyramidVector.Cross(worldBase[2]).Cross(pyramidVector); normal.Normalize(); p1 = anchor + 32.0f * pyramidVector - body1->GetWorldOrigin(); J1row.SubVec3(0) = normal; - J1row.SubVec3(1) = p1.Cross( normal ); - J1.Set( 1, 6, J1row.ToFloatPtr() ); + J1row.SubVec3(1) = p1.Cross(normal); + J1.Set(1, 6, J1row.ToFloatPtr()); - c1[0] = (invTimeStep * LIMIT_ERROR_REDUCTION) * ( normal * (32.0f * body1ax) ); + c1[0] = (invTimeStep * LIMIT_ERROR_REDUCTION) * (normal * (32.0f * body1ax)); - if ( body2 ) { + if (body2) { p2 = anchor + 32.0f * pyramidVector - master->GetWorldOrigin(); J2row.SubVec3(0) = -normal; - J2row.SubVec3(1) = p2.Cross( -normal ); - J2.Set( 1, 6, J2row.ToFloatPtr() ); + J2row.SubVec3(1) = p2.Cross(-normal); + J2.Set(1, 6, J2row.ToFloatPtr()); c2[0] = 0.0f; } @@ -3753,7 +3757,7 @@ bool idAFConstraint_PyramidLimit::Add( idPhysics_AF *phys, float invTimeStep ) { lo[0] = 0.0f; e[0] = LIMIT_LCP_EPSILON; - physics->AddFrameConstraint( this ); + physics->AddFrameConstraint(this); return true; } @@ -3763,8 +3767,8 @@ bool idAFConstraint_PyramidLimit::Add( idPhysics_AF *phys, float invTimeStep ) { idAFConstraint_PyramidLimit::Translate ================ */ -void idAFConstraint_PyramidLimit::Translate( const idVec3 &translation ) { - if ( !body2 ) { +void idAFConstraint_PyramidLimit::Translate(const idVec3& translation) { + if (!body2) { pyramidAnchor += translation; } } @@ -3774,8 +3778,8 @@ void idAFConstraint_PyramidLimit::Translate( const idVec3 &translation ) { idAFConstraint_PyramidLimit::Rotate ================ */ -void idAFConstraint_PyramidLimit::Rotate( const idRotation &rotation ) { - if ( !body2 ) { +void idAFConstraint_PyramidLimit::Rotate(const idRotation& rotation) { + if (!body2) { pyramidAnchor *= rotation; pyramidBasis[0] *= rotation.ToMat3(); pyramidBasis[1] *= rotation.ToMat3(); @@ -3788,17 +3792,17 @@ void idAFConstraint_PyramidLimit::Rotate( const idRotation &rotation ) { idAFConstraint_PyramidLimit::DebugDraw ================ */ -void idAFConstraint_PyramidLimit::DebugDraw( void ) { +void idAFConstraint_PyramidLimit::DebugDraw(void) { int i; float size = 10.0f; idVec3 anchor, dir, p[4]; idMat3 worldBase, m[2]; idQuat q; - idAFBody *master; + idAFBody* master; master = body2 ? body2 : physics->GetMasterBody(); - if ( master ) { + if (master) { worldBase[0] = pyramidBasis[0] * master->GetWorldAxis(); worldBase[1] = pyramidBasis[1] * master->GetWorldAxis(); worldBase[2] = pyramidBasis[2] * master->GetWorldAxis(); @@ -3810,10 +3814,10 @@ void idAFConstraint_PyramidLimit::DebugDraw( void ) { } // draw body1 axis - gameRenderWorld->DebugLine( colorGreen, anchor, anchor + size * (body1Axis * body1->GetWorldAxis()) ); + gameRenderWorld->DebugLine(colorGreen, anchor, anchor + size * (body1Axis * body1->GetWorldAxis())); // draw the pyramid - for ( i = 0; i < 2; i++ ) { + for (i = 0; i < 2; i++) { q.x = worldBase[!i].x * sinHalfAngle[i]; q.y = worldBase[!i].y * sinHalfAngle[i]; q.z = worldBase[!i].z * sinHalfAngle[i]; @@ -3827,9 +3831,9 @@ void idAFConstraint_PyramidLimit::DebugDraw( void ) { p[2] = anchor + m[0].Transpose() * (m[1].Transpose() * dir); p[3] = anchor + m[0].Transpose() * (m[1] * dir); - for ( i = 0; i < 4; i++ ) { - gameRenderWorld->DebugLine( colorMagenta, anchor, p[i] ); - gameRenderWorld->DebugLine( colorMagenta, p[i], p[(i+1)&3] ); + for (i = 0; i < 4; i++) { + gameRenderWorld->DebugLine(colorMagenta, anchor, p[i]); + gameRenderWorld->DebugLine(colorMagenta, p[i], p[(i + 1) & 3]); } } @@ -3838,18 +3842,18 @@ void idAFConstraint_PyramidLimit::DebugDraw( void ) { idAFConstraint_PyramidLimit::Save ================ */ -void idAFConstraint_PyramidLimit::Save( idSaveGame *saveFile ) const { - idAFConstraint::Save( saveFile ); - saveFile->WriteVec3( pyramidAnchor ); - saveFile->WriteMat3( pyramidBasis ); - saveFile->WriteVec3( body1Axis ); - saveFile->WriteFloat( cosAngle[0] ); - saveFile->WriteFloat( cosAngle[1] ); - saveFile->WriteFloat( sinHalfAngle[0] ); - saveFile->WriteFloat( sinHalfAngle[1] ); - saveFile->WriteFloat( cosHalfAngle[0] ); - saveFile->WriteFloat( cosHalfAngle[1] ); - saveFile->WriteFloat( epsilon ); +void idAFConstraint_PyramidLimit::Save(idSaveGame* saveFile) const { + idAFConstraint::Save(saveFile); + saveFile->WriteVec3(pyramidAnchor); + saveFile->WriteMat3(pyramidBasis); + saveFile->WriteVec3(body1Axis); + saveFile->WriteFloat(cosAngle[0]); + saveFile->WriteFloat(cosAngle[1]); + saveFile->WriteFloat(sinHalfAngle[0]); + saveFile->WriteFloat(sinHalfAngle[1]); + saveFile->WriteFloat(cosHalfAngle[0]); + saveFile->WriteFloat(cosHalfAngle[1]); + saveFile->WriteFloat(epsilon); } /* @@ -3857,18 +3861,18 @@ void idAFConstraint_PyramidLimit::Save( idSaveGame *saveFile ) const { idAFConstraint_PyramidLimit::Restore ================ */ -void idAFConstraint_PyramidLimit::Restore( idRestoreGame *saveFile ) { - idAFConstraint::Restore( saveFile ); - saveFile->ReadVec3( pyramidAnchor ); - saveFile->ReadMat3( pyramidBasis ); - saveFile->ReadVec3( body1Axis ); - saveFile->ReadFloat( cosAngle[0] ); - saveFile->ReadFloat( cosAngle[1] ); - saveFile->ReadFloat( sinHalfAngle[0] ); - saveFile->ReadFloat( sinHalfAngle[1] ); - saveFile->ReadFloat( cosHalfAngle[0] ); - saveFile->ReadFloat( cosHalfAngle[1] ); - saveFile->ReadFloat( epsilon ); +void idAFConstraint_PyramidLimit::Restore(idRestoreGame* saveFile) { + idAFConstraint::Restore(saveFile); + saveFile->ReadVec3(pyramidAnchor); + saveFile->ReadMat3(pyramidBasis); + saveFile->ReadVec3(body1Axis); + saveFile->ReadFloat(cosAngle[0]); + saveFile->ReadFloat(cosAngle[1]); + saveFile->ReadFloat(sinHalfAngle[0]); + saveFile->ReadFloat(sinHalfAngle[1]); + saveFile->ReadFloat(cosHalfAngle[0]); + saveFile->ReadFloat(cosHalfAngle[1]); + saveFile->ReadFloat(epsilon); } @@ -3883,10 +3887,10 @@ void idAFConstraint_PyramidLimit::Restore( idRestoreGame *saveFile ) { idAFConstraint_Suspension::idAFConstraint_Suspension ================ */ -idAFConstraint_Suspension::idAFConstraint_Suspension( void ) { +idAFConstraint_Suspension::idAFConstraint_Suspension(void) { type = CONSTRAINT_SUSPENSION; name = "suspension"; - InitSize( 3 ); + InitSize(3); fl.allowPrimary = false; fl.frameConstraint = true; @@ -3902,7 +3906,7 @@ idAFConstraint_Suspension::idAFConstraint_Suspension( void ) { motorForce = 0.0f; motorVelocity = 0.0f; wheelModel = NULL; - memset( &trace, 0, sizeof( trace ) ); + memset(&trace, 0, sizeof(trace)); epsilon = LCP_EPSILON; } @@ -3911,11 +3915,11 @@ idAFConstraint_Suspension::idAFConstraint_Suspension( void ) { idAFConstraint_Suspension::Setup ================ */ -void idAFConstraint_Suspension::Setup( const char *name, idAFBody *body, const idVec3 &origin, const idMat3 &axis, idClipModel *clipModel ) { +void idAFConstraint_Suspension::Setup(const char* name, idAFBody* body, const idVec3& origin, const idMat3& axis, idClipModel* clipModel) { this->name = name; body1 = body; body2 = NULL; - localOrigin = ( origin - body->GetWorldOrigin() ) * body->GetWorldAxis().Transpose(); + localOrigin = (origin - body->GetWorldOrigin()) * body->GetWorldAxis().Transpose(); localAxis = axis * body->GetWorldAxis().Transpose(); wheelModel = clipModel; } @@ -3925,7 +3929,7 @@ void idAFConstraint_Suspension::Setup( const char *name, idAFBody *body, const i idAFConstraint_Suspension::SetSuspension ================ */ -void idAFConstraint_Suspension::SetSuspension( const float up, const float down, const float k, const float d, const float f ) { +void idAFConstraint_Suspension::SetSuspension(const float up, const float down, const float k, const float d, const float f) { suspensionUp = up; suspensionDown = down; suspensionKCompress = k; @@ -3938,7 +3942,7 @@ void idAFConstraint_Suspension::SetSuspension( const float up, const float down, idAFConstraint_Suspension::GetWheelOrigin ================ */ -const idVec3 idAFConstraint_Suspension::GetWheelOrigin( void ) const { +const idVec3 idAFConstraint_Suspension::GetWheelOrigin(void) const { return body1->GetWorldOrigin() + wheelOffset * body1->GetWorldAxis(); } @@ -3947,7 +3951,7 @@ const idVec3 idAFConstraint_Suspension::GetWheelOrigin( void ) const { idAFConstraint_Suspension::Evaluate ================ */ -void idAFConstraint_Suspension::Evaluate( float invTimeStep ) { +void idAFConstraint_Suspension::Evaluate(float invTimeStep) { float velocity, suspensionLength, springLength, compression, dampingForce, springForce; idVec3 origin, start, end, vel1, vel2, springDir, r, frictionDir, motorDir; idMat3 axis; @@ -3958,52 +3962,53 @@ void idAFConstraint_Suspension::Evaluate( float invTimeStep ) { start = origin + suspensionUp * axis[2]; end = origin - suspensionDown * axis[2]; - rotation.SetVec( axis[2] ); - rotation.SetAngle( steerAngle ); + rotation.SetVec(axis[2]); + rotation.SetAngle(steerAngle); axis *= rotation.ToMat3(); - gameLocal.clip.Translation( trace, start, end, wheelModel, axis, MASK_SOLID, NULL ); + gameLocal.clip.Translation(trace, start, end, wheelModel, axis, CONTENTS_SOLID, NULL); - wheelOffset = ( trace.endpos - body1->GetWorldOrigin() ) * body1->GetWorldAxis().Transpose(); + wheelOffset = (trace.endpos - body1->GetWorldOrigin()) * body1->GetWorldAxis().Transpose(); - if ( trace.fraction >= 1.0f ) { - J1.SetSize( 0, 6 ); - if ( body2 ) { - J2.SetSize( 0, 6 ); + if (trace.fraction >= 1.0f) { + J1.SetSize(0, 6); + if (body2) { + J2.SetSize(0, 6); } return; } // calculate and add spring force - vel1 = body1->GetPointVelocity( start ); - if ( body2 ) { - vel2 = body2->GetPointVelocity( trace.c.point ); - } else { + vel1 = body1->GetPointVelocity(start); + if (body2) { + vel2 = body2->GetPointVelocity(trace.c.point); + } + else { vel2.Zero(); } suspensionLength = suspensionUp + suspensionDown; springDir = trace.endpos - start; springLength = trace.fraction * suspensionLength; - dampingForce = suspensionDamping * idMath::Fabs( ( vel2 - vel1 ) * springDir ) / ( 1.0f + springLength * springLength ); + dampingForce = suspensionDamping * idMath::Fabs((vel2 - vel1) * springDir) / (1.0f + springLength * springLength); compression = suspensionLength - springLength; springForce = compression * compression * suspensionKCompress - dampingForce; r = trace.c.point - body1->GetWorldOrigin(); - J1.SetSize( 2, 6 ); + J1.SetSize(2, 6); J1.SubVec6(0).SubVec3(0) = trace.c.normal; - J1.SubVec6(0).SubVec3(1) = r.Cross( trace.c.normal ); - c1.SetSize( 2 ); + J1.SubVec6(0).SubVec3(1) = r.Cross(trace.c.normal); + c1.SetSize(2); c1[0] = 0.0f; velocity = J1.SubVec6(0).SubVec3(0) * body1->GetLinearVelocity() + J1.SubVec6(0).SubVec3(1) * body1->GetAngularVelocity(); - if ( body2 ) { + if (body2) { r = trace.c.point - body2->GetWorldOrigin(); - J2.SetSize( 2, 6 ); + J2.SetSize(2, 6); J2.SubVec6(0).SubVec3(0) = -trace.c.normal; - J2.SubVec6(0).SubVec3(1) = r.Cross( -trace.c.normal ); - c2.SetSize( 2 ); + J2.SubVec6(0).SubVec3(1) = r.Cross(-trace.c.normal); + c2.SetSize(2); c2[0] = 0.0f; velocity += J2.SubVec6(0).SubVec3(0) * body2->GetLinearVelocity() + J2.SubVec6(0).SubVec3(1) * body2->GetAngularVelocity(); } @@ -4023,14 +4028,14 @@ void idAFConstraint_Suspension::Evaluate( float invTimeStep ) { r = trace.c.point - body1->GetWorldOrigin(); J1.SubVec6(1).SubVec3(0) = frictionDir; - J1.SubVec6(1).SubVec3(1) = r.Cross( frictionDir ); + J1.SubVec6(1).SubVec3(1) = r.Cross(frictionDir); c1[1] = 0.0f; - if ( body2 ) { + if (body2) { r = trace.c.point - body2->GetWorldOrigin(); J2.SubVec6(1).SubVec3(0) = -frictionDir; - J2.SubVec6(1).SubVec3(1) = r.Cross( -frictionDir ); + J2.SubVec6(1).SubVec3(1) = r.Cross(-frictionDir); c2[1] = 0.0f; } @@ -4041,26 +4046,26 @@ void idAFConstraint_Suspension::Evaluate( float invTimeStep ) { boxIndex[1] = 0; - if ( motorEnabled ) { + if (motorEnabled) { // project the motor force direction into the contact plane motorDir = axis[0] - axis[0] * trace.c.normal * axis[0]; motorDir.Normalize(); r = trace.c.point - body1->GetWorldOrigin(); - J1.ChangeSize( 3, J1.GetNumColumns() ); + J1.ChangeSize(3, J1.GetNumColumns()); J1.SubVec6(2).SubVec3(0) = -motorDir; - J1.SubVec6(2).SubVec3(1) = r.Cross( -motorDir ); - c1.ChangeSize( 3 ); + J1.SubVec6(2).SubVec3(1) = r.Cross(-motorDir); + c1.ChangeSize(3); c1[2] = motorVelocity; - if ( body2 ) { + if (body2) { r = trace.c.point - body2->GetWorldOrigin(); - J2.ChangeSize( 3, J2.GetNumColumns() ); + J2.ChangeSize(3, J2.GetNumColumns()); J2.SubVec6(2).SubVec3(0) = -motorDir; - J2.SubVec6(2).SubVec3(1) = r.Cross( -motorDir ); - c2.ChangeSize( 3 ); + J2.SubVec6(2).SubVec3(1) = r.Cross(-motorDir); + c2.ChangeSize(3); c2[2] = 0.0f; } @@ -4075,7 +4080,7 @@ void idAFConstraint_Suspension::Evaluate( float invTimeStep ) { idAFConstraint_Suspension::ApplyFriction ================ */ -void idAFConstraint_Suspension::ApplyFriction( float invTimeStep ) { +void idAFConstraint_Suspension::ApplyFriction(float invTimeStep) { // do nothing } @@ -4084,7 +4089,7 @@ void idAFConstraint_Suspension::ApplyFriction( float invTimeStep ) { idAFConstraint_Suspension::Translate ================ */ -void idAFConstraint_Suspension::Translate( const idVec3 &translation ) { +void idAFConstraint_Suspension::Translate(const idVec3& translation) { } /* @@ -4092,7 +4097,7 @@ void idAFConstraint_Suspension::Translate( const idVec3 &translation ) { idAFConstraint_Suspension::Rotate ================ */ -void idAFConstraint_Suspension::Rotate( const idRotation &rotation ) { +void idAFConstraint_Suspension::Rotate(const idRotation& rotation) { } /* @@ -4100,24 +4105,24 @@ void idAFConstraint_Suspension::Rotate( const idRotation &rotation ) { idAFConstraint_Suspension::DebugDraw ================ */ -void idAFConstraint_Suspension::DebugDraw( void ) { +void idAFConstraint_Suspension::DebugDraw(void) { idVec3 origin; idMat3 axis; idRotation rotation; axis = localAxis * body1->GetWorldAxis(); - rotation.SetVec( axis[2] ); - rotation.SetAngle( steerAngle ); + rotation.SetVec(axis[2]); + rotation.SetAngle(steerAngle); axis *= rotation.ToMat3(); - if ( trace.fraction < 1.0f ) { + if (trace.fraction < 1.0f) { origin = trace.c.point; - gameRenderWorld->DebugLine( colorWhite, origin, origin + 6.0f * axis[2] ); - gameRenderWorld->DebugLine( colorWhite, origin - 4.0f * axis[0], origin + 4.0f * axis[0] ); - gameRenderWorld->DebugLine( colorWhite, origin - 2.0f * axis[1], origin + 2.0f * axis[1] ); + gameRenderWorld->DebugLine(colorWhite, origin, origin + 6.0f * axis[2]); + gameRenderWorld->DebugLine(colorWhite, origin - 4.0f * axis[0], origin + 4.0f * axis[0]); + gameRenderWorld->DebugLine(colorWhite, origin - 2.0f * axis[1], origin + 2.0f * axis[1]); } } @@ -4133,7 +4138,7 @@ void idAFConstraint_Suspension::DebugDraw( void ) { idAFBody::idAFBody ================ */ -idAFBody::idAFBody( void ) { +idAFBody::idAFBody(void) { Init(); } @@ -4142,18 +4147,18 @@ idAFBody::idAFBody( void ) { idAFBody::idAFBody ================ */ -idAFBody::idAFBody( const idStr &name, idClipModel *clipModel, float density ) { +idAFBody::idAFBody(const idStr& name, idClipModel* clipModel, float density) { - assert( clipModel ); - assert( clipModel->IsTraceModel() ); + assert(clipModel); + assert(clipModel->IsTraceModel()); Init(); this->name = name; this->clipModel = NULL; - SetClipModel( clipModel ); - SetDensity( density ); + SetClipModel(clipModel); + SetDensity(density); current->worldOrigin = clipModel->GetOrigin(); current->worldAxis = clipModel->GetAxis(); @@ -4166,7 +4171,7 @@ idAFBody::idAFBody( const idStr &name, idClipModel *clipModel, float density ) { idAFBody::~idAFBody ================ */ -idAFBody::~idAFBody( void ) { +idAFBody::~idAFBody(void) { delete clipModel; } @@ -4175,56 +4180,56 @@ idAFBody::~idAFBody( void ) { idAFBody::Init ================ */ -void idAFBody::Init( void ) { - name = "noname"; - parent = NULL; - clipModel = NULL; - primaryConstraint = NULL; - tree = NULL; +void idAFBody::Init(void) { + name = "noname"; + parent = NULL; + clipModel = NULL; + primaryConstraint = NULL; + tree = NULL; - linearFriction = -1.0f; - angularFriction = -1.0f; - contactFriction = -1.0f; - bouncyness = -1.0f; - clipMask = 0; + linearFriction = -1.0f; + angularFriction = -1.0f; + contactFriction = -1.0f; + bouncyness = -1.0f; + clipMask = 0; - frictionDir = vec3_zero; - contactMotorDir = vec3_zero; - contactMotorVelocity = 0.0f; - contactMotorForce = 0.0f; + frictionDir = vec3_zero; + contactMotorDir = vec3_zero; + contactMotorVelocity = 0.0f; + contactMotorForce = 0.0f; - mass = 1.0f; - invMass = 1.0f; - centerOfMass = vec3_zero; - inertiaTensor = mat3_identity; - inverseInertiaTensor = mat3_identity; + mass = 1.0f; + invMass = 1.0f; + centerOfMass = vec3_zero; + inertiaTensor = mat3_identity; + inverseInertiaTensor = mat3_identity; - current = &state[0]; - next = &state[1]; - current->worldOrigin = vec3_zero; - current->worldAxis = mat3_identity; - current->spatialVelocity = vec6_zero; - current->externalForce = vec6_zero; - *next = *current; - saved = *current; - atRestOrigin = vec3_zero; - atRestAxis = mat3_identity; + current = &state[0]; + next = &state[1]; + current->worldOrigin = vec3_zero; + current->worldAxis = mat3_identity; + current->spatialVelocity = vec6_zero; + current->externalForce = vec6_zero; + *next = *current; + saved = *current; + atRestOrigin = vec3_zero; + atRestAxis = mat3_identity; - s.Zero( 6 ); - totalForce.Zero( 6 ); - auxForce.Zero( 6 ); - acceleration.Zero( 6 ); + s.Zero(6); + totalForce.Zero(6); + auxForce.Zero(6); + acceleration.Zero(6); - response = NULL; - responseIndex = NULL; - numResponses = 0; - maxAuxiliaryIndex = 0; - maxSubTreeAuxiliaryIndex = 0; + response = NULL; + responseIndex = NULL; + numResponses = 0; + maxAuxiliaryIndex = 0; + maxSubTreeAuxiliaryIndex = 0; - memset( &fl, 0, sizeof( fl ) ); + memset(&fl, 0, sizeof(fl)); - fl.selfCollision = true; - fl.isZero = true; + fl.selfCollision = true; + fl.isZero = true; } /* @@ -4232,8 +4237,8 @@ void idAFBody::Init( void ) { idAFBody::SetClipModel ================ */ -void idAFBody::SetClipModel( idClipModel *clipModel ) { - if ( this->clipModel && this->clipModel != clipModel ) { +void idAFBody::SetClipModel(idClipModel* clipModel) { + if (this->clipModel && this->clipModel != clipModel) { delete this->clipModel; } this->clipModel = clipModel; @@ -4244,11 +4249,11 @@ void idAFBody::SetClipModel( idClipModel *clipModel ) { idAFBody::SetFriction ================ */ -void idAFBody::SetFriction( float linear, float angular, float contact ) { - if ( linear < 0.0f || linear > 1.0f || - angular < 0.0f || angular > 1.0f || - contact < 0.0f ) { - gameLocal.Warning( "idAFBody::SetFriction: friction out of range, linear = %.1f, angular = %.1f, contact = %.1f", linear, angular, contact ); +void idAFBody::SetFriction(float linear, float angular, float contact) { + if (linear < 0.0f || linear > 1.0f || + angular < 0.0f || angular > 1.0f || + contact < 0.0f) { + gameLocal.Warning("idAFBody::SetFriction: friction out of range, linear = %.1f, angular = %.1f, contact = %.1f", linear, angular, contact); return; } linearFriction = linear; @@ -4261,9 +4266,9 @@ void idAFBody::SetFriction( float linear, float angular, float contact ) { idAFBody::SetBouncyness ================ */ -void idAFBody::SetBouncyness( float bounce ) { - if ( bounce < 0.0f || bounce > 1.0f ) { - gameLocal.Warning( "idAFBody::SetBouncyness: bouncyness out of range, bounce = %.1f", bounce ); +void idAFBody::SetBouncyness(float bounce) { + if (bounce < 0.0f || bounce > 1.0f) { + gameLocal.Warning("idAFBody::SetBouncyness: bouncyness out of range, bounce = %.1f", bounce); return; } bouncyness = bounce; @@ -4274,31 +4279,31 @@ void idAFBody::SetBouncyness( float bounce ) { idAFBody::SetDensity ================ */ -void idAFBody::SetDensity( float density, const idMat3 &inertiaScale ) { +void idAFBody::SetDensity(float density, const idMat3& inertiaScale) { // get the body mass properties - clipModel->GetMassProperties( density, mass, centerOfMass, inertiaTensor ); + clipModel->GetMassProperties(density, mass, centerOfMass, inertiaTensor); // make sure we have a valid mass - if ( mass <= 0.0f || FLOAT_IS_NAN( mass ) ) { - gameLocal.Warning( "idAFBody::SetDensity: invalid mass for body '%s'", name.c_str() ); + if (mass < idMath::FLT_EPSILON || FLOAT_IS_NAN(mass)) { + gameLocal.Warning("idAFBody::SetDensity: invalid mass for body '%s'", name.c_str()); mass = 1.0f; centerOfMass.Zero(); inertiaTensor.Identity(); } // make sure the center of mass is at the body origin - if ( !centerOfMass.Compare( vec3_origin, CENTER_OF_MASS_EPSILON ) ) { - gameLocal.Warning( "idAFBody::SetDentity: center of mass not at origin for body '%s'", name.c_str() ); + if (!centerOfMass.Compare(vec3_origin, CENTER_OF_MASS_EPSILON)) { + gameLocal.Warning("idAFBody::SetDentity: center of mass not at origin for body '%s'", name.c_str()); } centerOfMass.Zero(); // calculate the inverse mass and inverse inertia tensor invMass = 1.0f / mass; - if ( inertiaScale != mat3_identity ) { + if (inertiaScale != mat3_identity) { inertiaTensor *= inertiaScale; } - if ( inertiaTensor.IsDiagonal( 1e-3f ) ) { + if (inertiaTensor.IsDiagonal(1e-3f)) { inertiaTensor[0][1] = inertiaTensor[0][2] = 0.0f; inertiaTensor[1][0] = inertiaTensor[1][2] = 0.0f; inertiaTensor[2][0] = inertiaTensor[2][1] = 0.0f; @@ -4317,7 +4322,7 @@ void idAFBody::SetDensity( float density, const idMat3 &inertiaScale ) { idAFBody::SetFrictionDirection ================ */ -void idAFBody::SetFrictionDirection( const idVec3 &dir ) { +void idAFBody::SetFrictionDirection(const idVec3& dir) { frictionDir = dir * current->worldAxis.Transpose(); fl.useFrictionDir = true; } @@ -4327,8 +4332,8 @@ void idAFBody::SetFrictionDirection( const idVec3 &dir ) { idAFBody::GetFrictionDirection ================ */ -bool idAFBody::GetFrictionDirection( idVec3 &dir ) const { - if ( fl.useFrictionDir ) { +bool idAFBody::GetFrictionDirection(idVec3& dir) const { + if (fl.useFrictionDir) { dir = frictionDir * current->worldAxis; return true; } @@ -4340,7 +4345,7 @@ bool idAFBody::GetFrictionDirection( idVec3 &dir ) const { idAFBody::SetContactMotorDirection ================ */ -void idAFBody::SetContactMotorDirection( const idVec3 &dir ) { +void idAFBody::SetContactMotorDirection(const idVec3& dir) { contactMotorDir = dir * current->worldAxis.Transpose(); fl.useContactMotorDir = true; } @@ -4350,8 +4355,8 @@ void idAFBody::SetContactMotorDirection( const idVec3 &dir ) { idAFBody::GetContactMotorDirection ================ */ -bool idAFBody::GetContactMotorDirection( idVec3 &dir ) const { - if ( fl.useContactMotorDir ) { +bool idAFBody::GetContactMotorDirection(idVec3& dir) const { + if (fl.useContactMotorDir) { dir = contactMotorDir * current->worldAxis; return true; } @@ -4363,9 +4368,9 @@ bool idAFBody::GetContactMotorDirection( idVec3 &dir ) const { idAFBody::GetPointVelocity ================ */ -idVec3 idAFBody::GetPointVelocity( const idVec3 &point ) const { +idVec3 idAFBody::GetPointVelocity(const idVec3& point) const { idVec3 r = point - current->worldOrigin; - return current->spatialVelocity.SubVec3(0) + current->spatialVelocity.SubVec3(1).Cross( r ); + return current->spatialVelocity.SubVec3(0) + current->spatialVelocity.SubVec3(1).Cross(r); } /* @@ -4373,9 +4378,9 @@ idVec3 idAFBody::GetPointVelocity( const idVec3 &point ) const { idAFBody::AddForce ================ */ -void idAFBody::AddForce( const idVec3 &point, const idVec3 &force ) { +void idAFBody::AddForce(const idVec3& point, const idVec3& force) { current->externalForce.SubVec3(0) += force; - current->externalForce.SubVec3(1) += (point - current->worldOrigin).Cross( force ); + current->externalForce.SubVec3(1) += (point - current->worldOrigin).Cross(force); } /* @@ -4385,20 +4390,21 @@ idAFBody::InverseWorldSpatialInertiaMultiply dst = this->inverseWorldSpatialInertia * v; ================ */ -ID_INLINE void idAFBody::InverseWorldSpatialInertiaMultiply( idVecX &dst, const float *v ) const { - const float *mPtr = inverseWorldSpatialInertia.ToFloatPtr(); - const float *vPtr = v; - float *dstPtr = dst.ToFloatPtr(); +ID_INLINE void idAFBody::InverseWorldSpatialInertiaMultiply(idVecX& dst, const float* v) const { + const float* mPtr = inverseWorldSpatialInertia.ToFloatPtr(); + const float* vPtr = v; + float* dstPtr = dst.ToFloatPtr(); - if ( fl.spatialInertiaSparse ) { - dstPtr[0] = mPtr[0*6+0] * vPtr[0]; - dstPtr[1] = mPtr[1*6+1] * vPtr[1]; - dstPtr[2] = mPtr[2*6+2] * vPtr[2]; - dstPtr[3] = mPtr[3*6+3] * vPtr[3] + mPtr[3*6+4] * vPtr[4] + mPtr[3*6+5] * vPtr[5]; - dstPtr[4] = mPtr[4*6+3] * vPtr[3] + mPtr[4*6+4] * vPtr[4] + mPtr[4*6+5] * vPtr[5]; - dstPtr[5] = mPtr[5*6+3] * vPtr[3] + mPtr[5*6+4] * vPtr[4] + mPtr[5*6+5] * vPtr[5]; - } else { - gameLocal.Warning( "spatial inertia is not sparse for body %s", name.c_str() ); + if (fl.spatialInertiaSparse) { + dstPtr[0] = mPtr[0 * 6 + 0] * vPtr[0]; + dstPtr[1] = mPtr[1 * 6 + 1] * vPtr[1]; + dstPtr[2] = mPtr[2 * 6 + 2] * vPtr[2]; + dstPtr[3] = mPtr[3 * 6 + 3] * vPtr[3] + mPtr[3 * 6 + 4] * vPtr[4] + mPtr[3 * 6 + 5] * vPtr[5]; + dstPtr[4] = mPtr[4 * 6 + 3] * vPtr[3] + mPtr[4 * 6 + 4] * vPtr[4] + mPtr[4 * 6 + 5] * vPtr[5]; + dstPtr[5] = mPtr[5 * 6 + 3] * vPtr[3] + mPtr[5 * 6 + 4] * vPtr[4] + mPtr[5 * 6 + 5] * vPtr[5]; + } + else { + gameLocal.Warning("spatial inertia is not sparse for body %s", name.c_str()); } } @@ -4407,29 +4413,29 @@ ID_INLINE void idAFBody::InverseWorldSpatialInertiaMultiply( idVecX &dst, const idAFBody::Save ================ */ -void idAFBody::Save( idSaveGame *saveFile ) { - saveFile->WriteFloat( linearFriction ); - saveFile->WriteFloat( angularFriction ); - saveFile->WriteFloat( contactFriction ); - saveFile->WriteFloat( bouncyness ); - saveFile->WriteInt( clipMask ); - saveFile->WriteVec3( frictionDir ); - saveFile->WriteVec3( contactMotorDir ); - saveFile->WriteFloat( contactMotorVelocity ); - saveFile->WriteFloat( contactMotorForce ); +void idAFBody::Save(idSaveGame* saveFile) { + saveFile->WriteFloat(linearFriction); + saveFile->WriteFloat(angularFriction); + saveFile->WriteFloat(contactFriction); + saveFile->WriteFloat(bouncyness); + saveFile->WriteInt(clipMask); + saveFile->WriteVec3(frictionDir); + saveFile->WriteVec3(contactMotorDir); + saveFile->WriteFloat(contactMotorVelocity); + saveFile->WriteFloat(contactMotorForce); - saveFile->WriteFloat( mass ); - saveFile->WriteFloat( invMass ); - saveFile->WriteVec3( centerOfMass ); - saveFile->WriteMat3( inertiaTensor ); - saveFile->WriteMat3( inverseInertiaTensor ); + saveFile->WriteFloat(mass); + saveFile->WriteFloat(invMass); + saveFile->WriteVec3(centerOfMass); + saveFile->WriteMat3(inertiaTensor); + saveFile->WriteMat3(inverseInertiaTensor); - saveFile->WriteVec3( current->worldOrigin ); - saveFile->WriteMat3( current->worldAxis ); - saveFile->WriteVec6( current->spatialVelocity ); - saveFile->WriteVec6( current->externalForce ); - saveFile->WriteVec3( atRestOrigin ); - saveFile->WriteMat3( atRestAxis ); + saveFile->WriteVec3(current->worldOrigin); + saveFile->WriteMat3(current->worldAxis); + saveFile->WriteVec6(current->spatialVelocity); + saveFile->WriteVec6(current->externalForce); + saveFile->WriteVec3(atRestOrigin); + saveFile->WriteMat3(atRestAxis); } /* @@ -4437,29 +4443,29 @@ void idAFBody::Save( idSaveGame *saveFile ) { idAFBody::Restore ================ */ -void idAFBody::Restore( idRestoreGame *saveFile ) { - saveFile->ReadFloat( linearFriction ); - saveFile->ReadFloat( angularFriction ); - saveFile->ReadFloat( contactFriction ); - saveFile->ReadFloat( bouncyness ); - saveFile->ReadInt( clipMask ); - saveFile->ReadVec3( frictionDir ); - saveFile->ReadVec3( contactMotorDir ); - saveFile->ReadFloat( contactMotorVelocity ); - saveFile->ReadFloat( contactMotorForce ); +void idAFBody::Restore(idRestoreGame* saveFile) { + saveFile->ReadFloat(linearFriction); + saveFile->ReadFloat(angularFriction); + saveFile->ReadFloat(contactFriction); + saveFile->ReadFloat(bouncyness); + saveFile->ReadInt(clipMask); + saveFile->ReadVec3(frictionDir); + saveFile->ReadVec3(contactMotorDir); + saveFile->ReadFloat(contactMotorVelocity); + saveFile->ReadFloat(contactMotorForce); - saveFile->ReadFloat( mass ); - saveFile->ReadFloat( invMass ); - saveFile->ReadVec3( centerOfMass ); - saveFile->ReadMat3( inertiaTensor ); - saveFile->ReadMat3( inverseInertiaTensor ); + saveFile->ReadFloat(mass); + saveFile->ReadFloat(invMass); + saveFile->ReadVec3(centerOfMass); + saveFile->ReadMat3(inertiaTensor); + saveFile->ReadMat3(inverseInertiaTensor); - saveFile->ReadVec3( current->worldOrigin ); - saveFile->ReadMat3( current->worldAxis ); - saveFile->ReadVec6( current->spatialVelocity ); - saveFile->ReadVec6( current->externalForce ); - saveFile->ReadVec3( atRestOrigin ); - saveFile->ReadMat3( atRestAxis ); + saveFile->ReadVec3(current->worldOrigin); + saveFile->ReadMat3(current->worldAxis); + saveFile->ReadVec6(current->spatialVelocity); + saveFile->ReadVec6(current->externalForce); + saveFile->ReadVec3(atRestOrigin); + saveFile->ReadMat3(atRestAxis); } @@ -4477,49 +4483,49 @@ idAFTree::Factor factor matrix for the primary constraints in the tree ================ */ -void idAFTree::Factor( void ) const { +void idAFTree::Factor(void) const { int i, j; - idAFBody *body; - idAFConstraint *child; + idAFBody* body; + idAFConstraint* child; idMatX childI; - childI.SetData( 6, 6, MATX_ALLOCA( 6 * 6 ) ); + childI.SetData(6, 6, MATX_ALLOCA(6 * 6)); // from the leaves up towards the root - for ( i = sortedBodies.Num() - 1; i >= 0; i-- ) { + for (i = sortedBodies.Num() - 1; i >= 0; i--) { body = sortedBodies[i]; - if ( body->children.Num() ) { + if (body->children.Num()) { - for ( j = 0; j < body->children.Num(); j++ ) { + for (j = 0; j < body->children.Num(); j++) { child = body->children[j]->primaryConstraint; // child->I = - child->body1->J.Transpose() * child->body1->I * child->body1->J; - childI.SetSize( child->J1.GetNumRows(), child->J1.GetNumRows() ); - child->body1->J.TransposeMultiply( child->body1->I ).Multiply( childI, child->body1->J ); + childI.SetSize(child->J1.GetNumRows(), child->J1.GetNumRows()); + child->body1->J.TransposeMultiply(child->body1->I).Multiply(childI, child->body1->J); childI.Negate(); child->invI = childI; - if ( !child->invI.InverseFastSelf() ) { - gameLocal.Warning( "idAFTree::Factor: couldn't invert %dx%d matrix for constraint '%s'", - child->invI.GetNumRows(), child->invI.GetNumColumns(), child->GetName().c_str() ); + if (!child->invI.InverseFastSelf()) { + gameLocal.Warning("idAFTree::Factor: couldn't invert %dx%d matrix for constraint '%s'", + child->invI.GetNumRows(), child->invI.GetNumColumns(), child->GetName().c_str()); } child->J = child->invI * child->J; - body->I -= child->J.TransposeMultiply( childI ) * child->J; + body->I -= child->J.TransposeMultiply(childI) * child->J; } body->invI = body->I; - if ( !body->invI.InverseFastSelf() ) { - gameLocal.Warning( "idAFTree::Factor: couldn't invert %dx%d matrix for body %s", - child->invI.GetNumRows(), child->invI.GetNumColumns(), body->GetName().c_str() ); + if (!body->invI.InverseFastSelf()) { + gameLocal.Warning("idAFTree::Factor: couldn't invert %dx%d matrix for body %s", + child->invI.GetNumRows(), child->invI.GetNumColumns(), body->GetName().c_str()); } - if ( body->primaryConstraint ) { + if (body->primaryConstraint) { body->J = body->invI * body->J; } } - else if ( body->primaryConstraint ) { + else if (body->primaryConstraint) { body->J = body->inverseWorldSpatialInertia * body->J; } } @@ -4532,25 +4538,25 @@ idAFTree::Solve solve for primary constraints in the tree ================ */ -void idAFTree::Solve( int auxiliaryIndex ) const { +void idAFTree::Solve(int auxiliaryIndex) const { int i, j; - idAFBody *body, *child; - idAFConstraint *primaryConstraint; + idAFBody* body, * child; + idAFConstraint* primaryConstraint; // from the leaves up towards the root - for ( i = sortedBodies.Num() - 1; i >= 0; i-- ) { + for (i = sortedBodies.Num() - 1; i >= 0; i--) { body = sortedBodies[i]; - for ( j = 0; j < body->children.Num(); j++ ) { + for (j = 0; j < body->children.Num(); j++) { child = body->children[j]; primaryConstraint = child->primaryConstraint; - if ( !child->fl.isZero ) { - child->J.TransposeMultiplySub( primaryConstraint->s, child->s ); + if (!child->fl.isZero) { + child->J.TransposeMultiplySub(primaryConstraint->s, child->s); primaryConstraint->fl.isZero = false; } - if ( !primaryConstraint->fl.isZero ) { - primaryConstraint->J.TransposeMultiplySub( body->s, primaryConstraint->s ); + if (!primaryConstraint->fl.isZero) { + primaryConstraint->J.TransposeMultiplySub(body->s, primaryConstraint->s); body->fl.isZero = false; } } @@ -4559,34 +4565,35 @@ void idAFTree::Solve( int auxiliaryIndex ) const { bool useSymmetry = af_useSymmetry.GetBool(); // from the root down towards the leaves - for ( i = 0; i < sortedBodies.Num(); i++ ) { + for (i = 0; i < sortedBodies.Num(); i++) { body = sortedBodies[i]; primaryConstraint = body->primaryConstraint; - if ( primaryConstraint ) { + if (primaryConstraint) { - if ( useSymmetry && body->parent->maxSubTreeAuxiliaryIndex < auxiliaryIndex ) { + if (useSymmetry && body->parent->maxSubTreeAuxiliaryIndex < auxiliaryIndex) { continue; } - if ( !primaryConstraint->fl.isZero ) { + if (!primaryConstraint->fl.isZero) { primaryConstraint->s = primaryConstraint->invI * primaryConstraint->s; } - primaryConstraint->J.MultiplySub( primaryConstraint->s, primaryConstraint->body2->s ); + primaryConstraint->J.MultiplySub(primaryConstraint->s, primaryConstraint->body2->s); primaryConstraint->lm = primaryConstraint->s; - if ( useSymmetry && body->maxSubTreeAuxiliaryIndex < auxiliaryIndex ) { + if (useSymmetry && body->maxSubTreeAuxiliaryIndex < auxiliaryIndex) { continue; } - if ( body->children.Num() ) { - if ( !body->fl.isZero ) { + if (body->children.Num()) { + if (!body->fl.isZero) { body->s = body->invI * body->s; } - body->J.MultiplySub( body->s, primaryConstraint->s ); + body->J.MultiplySub(body->s, primaryConstraint->s); } - } else if ( body->children.Num() ) { + } + else if (body->children.Num()) { body->s = body->invI * body->s; } } @@ -4599,101 +4606,101 @@ idAFTree::Response calculate body forces in the tree in response to a constraint force ================ */ -void idAFTree::Response( const idAFConstraint *constraint, int row, int auxiliaryIndex ) const { +void idAFTree::Response(const idAFConstraint* constraint, int row, int auxiliaryIndex) const { int i, j; - idAFBody *body; - idAFConstraint *child, *primaryConstraint; + idAFBody* body; + idAFConstraint* child, * primaryConstraint; idVecX v; // if a single body don't waste time because there aren't any primary constraints - if ( sortedBodies.Num() == 1 ) { + if (sortedBodies.Num() == 1) { body = constraint->body1; - if ( body->tree == this ) { - body->GetResponseForce( body->numResponses ) = constraint->J1.SubVec6( row ); + if (body->tree == this) { + body->GetResponseForce(body->numResponses) = constraint->J1.SubVec6(row); body->responseIndex[body->numResponses++] = auxiliaryIndex; } else { body = constraint->body2; - body->GetResponseForce( body->numResponses ) = constraint->J2.SubVec6( row ); + body->GetResponseForce(body->numResponses) = constraint->J2.SubVec6(row); body->responseIndex[body->numResponses++] = auxiliaryIndex; } return; } - v.SetData( 6, VECX_ALLOCA( 6 ) ); + v.SetData(6, VECX_ALLOCA(6)); // initialize right hand side to zero - for ( i = 0; i < sortedBodies.Num(); i++ ) { + for (i = 0; i < sortedBodies.Num(); i++) { body = sortedBodies[i]; primaryConstraint = body->primaryConstraint; - if ( primaryConstraint ) { + if (primaryConstraint) { primaryConstraint->s.Zero(); primaryConstraint->fl.isZero = true; } body->s.Zero(); body->fl.isZero = true; - body->GetResponseForce( body->numResponses ).Zero(); + body->GetResponseForce(body->numResponses).Zero(); } // set right hand side for first constrained body body = constraint->body1; - if ( body->tree == this ) { - body->InverseWorldSpatialInertiaMultiply( v, constraint->J1[row] ); + if (body->tree == this) { + body->InverseWorldSpatialInertiaMultiply(v, constraint->J1[row]); primaryConstraint = body->primaryConstraint; - if ( primaryConstraint ) { - primaryConstraint->J1.Multiply( primaryConstraint->s, v ); + if (primaryConstraint) { + primaryConstraint->J1.Multiply(primaryConstraint->s, v); primaryConstraint->fl.isZero = false; } - for ( i = 0; i < body->children.Num(); i++ ) { + for (i = 0; i < body->children.Num(); i++) { child = body->children[i]->primaryConstraint; - child->J2.Multiply( child->s, v ); + child->J2.Multiply(child->s, v); child->fl.isZero = false; } - body->GetResponseForce( body->numResponses ) = constraint->J1.SubVec6( row ); + body->GetResponseForce(body->numResponses) = constraint->J1.SubVec6(row); } // set right hand side for second constrained body body = constraint->body2; - if ( body && body->tree == this ) { - body->InverseWorldSpatialInertiaMultiply( v, constraint->J2[row] ); + if (body && body->tree == this) { + body->InverseWorldSpatialInertiaMultiply(v, constraint->J2[row]); primaryConstraint = body->primaryConstraint; - if ( primaryConstraint ) { - primaryConstraint->J1.MultiplyAdd( primaryConstraint->s, v ); + if (primaryConstraint) { + primaryConstraint->J1.MultiplyAdd(primaryConstraint->s, v); primaryConstraint->fl.isZero = false; } - for ( i = 0; i < body->children.Num(); i++ ) { + for (i = 0; i < body->children.Num(); i++) { child = body->children[i]->primaryConstraint; - child->J2.MultiplyAdd( child->s, v ); + child->J2.MultiplyAdd(child->s, v); child->fl.isZero = false; } - body->GetResponseForce( body->numResponses ) = constraint->J2.SubVec6( row ); + body->GetResponseForce(body->numResponses) = constraint->J2.SubVec6(row); } // solve for primary constraints - Solve( auxiliaryIndex ); + Solve(auxiliaryIndex); bool useSymmetry = af_useSymmetry.GetBool(); // store body forces in response to the constraint force idVecX force; - for ( i = 0; i < sortedBodies.Num(); i++ ) { + for (i = 0; i < sortedBodies.Num(); i++) { body = sortedBodies[i]; - if ( useSymmetry && body->maxAuxiliaryIndex < auxiliaryIndex ) { + if (useSymmetry && body->maxAuxiliaryIndex < auxiliaryIndex) { continue; } - force.SetData( 6, body->response + body->numResponses * 8 ); + force.SetData(6, body->response + body->numResponses * 8); // add forces of all primary constraints acting on this body primaryConstraint = body->primaryConstraint; - if ( primaryConstraint ) { - primaryConstraint->J1.TransposeMultiplyAdd( force, primaryConstraint->lm ); + if (primaryConstraint) { + primaryConstraint->J1.TransposeMultiplyAdd(force, primaryConstraint->lm); } - for ( j = 0; j < body->children.Num(); j++ ) { + for (j = 0; j < body->children.Num(); j++) { child = body->children[j]->primaryConstraint; - child->J2.TransposeMultiplyAdd( force, child->lm ); + child->J2.TransposeMultiplyAdd(force, child->lm); } body->responseIndex[body->numResponses++] = auxiliaryIndex; @@ -4707,37 +4714,37 @@ idAFTree::CalculateForces calculate forces on the bodies in the tree ================ */ -void idAFTree::CalculateForces( float timeStep ) const { +void idAFTree::CalculateForces(float timeStep) const { int i, j; float invStep; - idAFBody *body; - idAFConstraint *child, *c, *primaryConstraint; + idAFBody* body; + idAFConstraint* child, * c, * primaryConstraint; // forces on bodies - for ( i = 0; i < sortedBodies.Num(); i++ ) { + for (i = 0; i < sortedBodies.Num(); i++) { body = sortedBodies[i]; body->totalForce.SubVec6(0) = body->current->externalForce + body->auxForce.SubVec6(0); } // if a single body don't waste time because there aren't any primary constraints - if ( sortedBodies.Num() == 1 ) { + if (sortedBodies.Num() == 1) { return; } invStep = 1.0f / timeStep; // initialize right hand side - for ( i = 0; i < sortedBodies.Num(); i++ ) { + for (i = 0; i < sortedBodies.Num(); i++) { body = sortedBodies[i]; - body->InverseWorldSpatialInertiaMultiply( body->acceleration, body->totalForce.ToFloatPtr() ); + body->InverseWorldSpatialInertiaMultiply(body->acceleration, body->totalForce.ToFloatPtr()); body->acceleration.SubVec6(0) += body->current->spatialVelocity * invStep; primaryConstraint = body->primaryConstraint; - if ( primaryConstraint ) { + if (primaryConstraint) { // b = ( J * acc + c ) c = primaryConstraint; - c->s = c->J1 * c->body1->acceleration + c->J2 * c->body2->acceleration + invStep * ( c->c1 + c->c2 ); + c->s = c->J1 * c->body1->acceleration + c->J2 * c->body2->acceleration + invStep * (c->c1 + c->c2); c->fl.isZero = false; } body->s.Zero(); @@ -4748,17 +4755,17 @@ void idAFTree::CalculateForces( float timeStep ) const { Solve(); // calculate forces on bodies after applying primary constraints - for ( i = 0; i < sortedBodies.Num(); i++ ) { + for (i = 0; i < sortedBodies.Num(); i++) { body = sortedBodies[i]; // add forces of all primary constraints acting on this body primaryConstraint = body->primaryConstraint; - if ( primaryConstraint ) { - primaryConstraint->J1.TransposeMultiplyAdd( body->totalForce, primaryConstraint->lm ); + if (primaryConstraint) { + primaryConstraint->J1.TransposeMultiplyAdd(body->totalForce, primaryConstraint->lm); } - for ( j = 0; j < body->children.Num(); j++ ) { + for (j = 0; j < body->children.Num(); j++) { child = body->children[j]->primaryConstraint; - child->J2.TransposeMultiplyAdd( body->totalForce, child->lm ); + child->J2.TransposeMultiplyAdd(body->totalForce, child->lm); } } } @@ -4768,18 +4775,18 @@ void idAFTree::CalculateForces( float timeStep ) const { idAFTree::SetMaxSubTreeAuxiliaryIndex ================ */ -void idAFTree::SetMaxSubTreeAuxiliaryIndex( void ) { +void idAFTree::SetMaxSubTreeAuxiliaryIndex(void) { int i, j; - idAFBody *body, *child; + idAFBody* body, * child; // from the leaves up towards the root - for ( i = sortedBodies.Num() - 1; i >= 0; i-- ) { + for (i = sortedBodies.Num() - 1; i >= 0; i--) { body = sortedBodies[i]; body->maxSubTreeAuxiliaryIndex = body->maxAuxiliaryIndex; - for ( j = 0; j < body->children.Num(); j++ ) { + for (j = 0; j < body->children.Num(); j++) { child = body->children[j]; - if ( child->maxSubTreeAuxiliaryIndex > body->maxSubTreeAuxiliaryIndex ) { + if (child->maxSubTreeAuxiliaryIndex > body->maxSubTreeAuxiliaryIndex) { body->maxSubTreeAuxiliaryIndex = child->maxSubTreeAuxiliaryIndex; } } @@ -4791,14 +4798,14 @@ void idAFTree::SetMaxSubTreeAuxiliaryIndex( void ) { idAFTree::SortBodies_r ================ */ -void idAFTree::SortBodies_r( idList&sortedList, idAFBody *body ) { +void idAFTree::SortBodies_r(idList& sortedList, idAFBody* body) { int i; - for ( i = 0; i < body->children.Num(); i++ ) { - sortedList.Append( body->children[i] ); + for (i = 0; i < body->children.Num(); i++) { + sortedList.Append(body->children[i]); } - for ( i = 0; i < body->children.Num(); i++ ) { - SortBodies_r( sortedList, body->children[i] ); + for (i = 0; i < body->children.Num(); i++) { + SortBodies_r(sortedList, body->children[i]); } } @@ -4809,25 +4816,25 @@ idAFTree::SortBodies sort body list to make sure parents come first ================ */ -void idAFTree::SortBodies( void ) { +void idAFTree::SortBodies(void) { int i; - idAFBody *body; + idAFBody* body; // find the root - for ( i = 0; i < sortedBodies.Num(); i++ ) { - if ( !sortedBodies[i]->parent ) { + for (i = 0; i < sortedBodies.Num(); i++) { + if (!sortedBodies[i]->parent) { break; } } - if ( i >= sortedBodies.Num() ) { - gameLocal.Error( "Articulated figure tree has no root." ); + if (i >= sortedBodies.Num()) { + gameLocal.Error("Articulated figure tree has no root."); } body = sortedBodies[i]; sortedBodies.Clear(); - sortedBodies.Append( body ); - SortBodies_r( sortedBodies, body ); + sortedBodies.Append(body); + SortBodies_r(sortedBodies, body); } /* @@ -4835,13 +4842,13 @@ void idAFTree::SortBodies( void ) { idAFTree::DebugDraw ================ */ -void idAFTree::DebugDraw( const idVec4 &color ) const { +void idAFTree::DebugDraw(const idVec4& color) const { int i; - idAFBody *body; + idAFBody* body; - for ( i = 1; i < sortedBodies.Num(); i++ ) { + for (i = 1; i < sortedBodies.Num(); i++) { body = sortedBodies[i]; - gameRenderWorld->DebugArrow( color, body->parent->current->worldOrigin, body->current->worldOrigin, 1 ); + gameRenderWorld->DebugArrow(color, body->parent->current->worldOrigin, body->current->worldOrigin, 1); } } @@ -4857,34 +4864,34 @@ void idAFTree::DebugDraw( const idVec4 &color ) const { idPhysics_AF::EvaluateConstraints ================ */ -void idPhysics_AF::EvaluateConstraints( float timeStep ) { +void idPhysics_AF::EvaluateConstraints(float timeStep) { int i; float invTimeStep; - idAFBody *body; - idAFConstraint *c; + idAFBody* body; + idAFConstraint* c; invTimeStep = 1.0f / timeStep; // setup the constraint equations for the current position and orientation of the bodies - for ( i = 0; i < primaryConstraints.Num(); i++ ) { + for (i = 0; i < primaryConstraints.Num(); i++) { c = primaryConstraints[i]; - c->Evaluate( invTimeStep ); + c->Evaluate(invTimeStep); c->J = c->J2; } - for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) { - auxiliaryConstraints[i]->Evaluate( invTimeStep ); + for (i = 0; i < auxiliaryConstraints.Num(); i++) { + auxiliaryConstraints[i]->Evaluate(invTimeStep); } // add contact constraints to the list with frame constraints - for ( i = 0; i < contactConstraints.Num(); i++ ) { - AddFrameConstraint( contactConstraints[i] ); + for (i = 0; i < contactConstraints.Num(); i++) { + AddFrameConstraint(contactConstraints[i]); } // setup body primary constraint matrix - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; - if ( body->primaryConstraint ) { + if (body->primaryConstraint) { body->J = body->primaryConstraint->J1.Transpose(); } } @@ -4895,36 +4902,36 @@ void idPhysics_AF::EvaluateConstraints( float timeStep ) { idPhysics_AF::EvaluateBodies ================ */ -void idPhysics_AF::EvaluateBodies( float timeStep ) { +void idPhysics_AF::EvaluateBodies(float timeStep) { int i; - idAFBody *body; + idAFBody* body; idMat3 axis; - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; // we transpose the axis before using it because idMat3 is column-major axis = body->current->worldAxis.Transpose(); // if the center of mass is at the body point of reference - if ( body->centerOfMass.Compare( vec3_origin, CENTER_OF_MASS_EPSILON ) ) { + if (body->centerOfMass.Compare(vec3_origin, CENTER_OF_MASS_EPSILON)) { // spatial inertia in world space - body->I.Set( body->mass * mat3_identity, mat3_zero, - mat3_zero, axis * body->inertiaTensor * axis.Transpose() ); + body->I.Set(body->mass * mat3_identity, mat3_zero, + mat3_zero, axis * body->inertiaTensor * axis.Transpose()); // inverse spatial inertia in world space - body->inverseWorldSpatialInertia.Set( body->invMass * mat3_identity, mat3_zero, - mat3_zero, axis * body->inverseInertiaTensor * axis.Transpose() ); + body->inverseWorldSpatialInertia.Set(body->invMass * mat3_identity, mat3_zero, + mat3_zero, axis * body->inverseInertiaTensor * axis.Transpose()); body->fl.spatialInertiaSparse = true; } else { - idMat3 massMoment = body->mass * SkewSymmetric( body->centerOfMass ); + idMat3 massMoment = body->mass * SkewSymmetric(body->centerOfMass); // spatial inertia in world space - body->I.Set( body->mass * mat3_identity, massMoment, - massMoment.Transpose(), axis * body->inertiaTensor * axis.Transpose() ); + body->I.Set(body->mass * mat3_identity, massMoment, + massMoment.Transpose(), axis * body->inertiaTensor * axis.Transpose()); // inverse spatial inertia in world space body->inverseWorldSpatialInertia = body->I.InverseFast(); @@ -4942,12 +4949,12 @@ void idPhysics_AF::EvaluateBodies( float timeStep ) { idPhysics_AF::AddFrameConstraints ================ */ -void idPhysics_AF::AddFrameConstraints( void ) { +void idPhysics_AF::AddFrameConstraints(void) { int i; // add frame constraints to auxiliary constraints - for ( i = 0; i < frameConstraints.Num(); i++ ) { - auxiliaryConstraints.Append( frameConstraints[i] ); + for (i = 0; i < frameConstraints.Num(); i++) { + auxiliaryConstraints.Append(frameConstraints[i]); } } @@ -4956,10 +4963,10 @@ void idPhysics_AF::AddFrameConstraints( void ) { idPhysics_AF::RemoveFrameConstraints ================ */ -void idPhysics_AF::RemoveFrameConstraints( void ) { +void idPhysics_AF::RemoveFrameConstraints(void) { // remove all the frame constraints from the auxiliary constraints - auxiliaryConstraints.SetNum( auxiliaryConstraints.Num() - frameConstraints.Num(), false ); - frameConstraints.SetNum( 0, false ); + auxiliaryConstraints.SetNum(auxiliaryConstraints.Num() - frameConstraints.Num(), false); + frameConstraints.SetNum(0, false); } /* @@ -4967,46 +4974,50 @@ void idPhysics_AF::RemoveFrameConstraints( void ) { idPhysics_AF::ApplyFriction ================ */ -void idPhysics_AF::ApplyFriction( float timeStep, float endTimeMSec ) { +void idPhysics_AF::ApplyFriction(float timeStep, float endTimeMSec) { int i; float invTimeStep; - if ( af_skipFriction.GetBool() ) { + if (af_skipFriction.GetBool()) { return; } - if ( jointFrictionDentStart < MS2SEC( endTimeMSec ) && jointFrictionDentEnd > MS2SEC( endTimeMSec ) ) { - float halfTime = ( jointFrictionDentEnd - jointFrictionDentStart ) * 0.5f; - if ( jointFrictionDentStart + halfTime > MS2SEC( endTimeMSec ) ) { - jointFrictionDentScale = 1.0f - ( 1.0f - jointFrictionDent ) * ( MS2SEC( endTimeMSec ) - jointFrictionDentStart ) / halfTime; - } else { - jointFrictionDentScale = jointFrictionDent + ( 1.0f - jointFrictionDent ) * ( MS2SEC( endTimeMSec ) - jointFrictionDentStart - halfTime ) / halfTime; + if (jointFrictionDentStart < MS2SEC(endTimeMSec) && jointFrictionDentEnd > MS2SEC(endTimeMSec)) { + float halfTime = (jointFrictionDentEnd - jointFrictionDentStart) * 0.5f; + if (jointFrictionDentStart + halfTime > MS2SEC(endTimeMSec)) { + jointFrictionDentScale = 1.0f - (1.0f - jointFrictionDent) * (MS2SEC(endTimeMSec) - jointFrictionDentStart) / halfTime; } - } else { + else { + jointFrictionDentScale = jointFrictionDent + (1.0f - jointFrictionDent) * (MS2SEC(endTimeMSec) - jointFrictionDentStart - halfTime) / halfTime; + } + } + else { jointFrictionDentScale = 0.0f; } - if ( contactFrictionDentStart < MS2SEC( endTimeMSec ) && contactFrictionDentEnd > MS2SEC( endTimeMSec ) ) { - float halfTime = ( contactFrictionDentEnd - contactFrictionDentStart ) * 0.5f; - if ( contactFrictionDentStart + halfTime > MS2SEC( endTimeMSec ) ) { - contactFrictionDentScale = 1.0f - ( 1.0f - contactFrictionDent ) * ( MS2SEC( endTimeMSec ) - contactFrictionDentStart ) / halfTime; - } else { - contactFrictionDentScale = contactFrictionDent + ( 1.0f - contactFrictionDent ) * ( MS2SEC( endTimeMSec ) - contactFrictionDentStart - halfTime ) / halfTime; + if (contactFrictionDentStart < MS2SEC(endTimeMSec) && contactFrictionDentEnd > MS2SEC(endTimeMSec)) { + float halfTime = (contactFrictionDentEnd - contactFrictionDentStart) * 0.5f; + if (contactFrictionDentStart + halfTime > MS2SEC(endTimeMSec)) { + contactFrictionDentScale = 1.0f - (1.0f - contactFrictionDent) * (MS2SEC(endTimeMSec) - contactFrictionDentStart) / halfTime; } - } else { + else { + contactFrictionDentScale = contactFrictionDent + (1.0f - contactFrictionDent) * (MS2SEC(endTimeMSec) - contactFrictionDentStart - halfTime) / halfTime; + } + } + else { contactFrictionDentScale = 0.0f; } invTimeStep = 1.0f / timeStep; - for ( i = 0; i < primaryConstraints.Num(); i++ ) { - primaryConstraints[i]->ApplyFriction( invTimeStep ); + for (i = 0; i < primaryConstraints.Num(); i++) { + primaryConstraints[i]->ApplyFriction(invTimeStep); } - for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) { - auxiliaryConstraints[i]->ApplyFriction( invTimeStep ); + for (i = 0; i < auxiliaryConstraints.Num(); i++) { + auxiliaryConstraints[i]->ApplyFriction(invTimeStep); } - for ( i = 0; i < frameConstraints.Num(); i++ ) { - frameConstraints[i]->ApplyFriction( invTimeStep ); + for (i = 0; i < frameConstraints.Num(); i++) { + frameConstraints[i]->ApplyFriction(invTimeStep); } } @@ -5015,10 +5026,10 @@ void idPhysics_AF::ApplyFriction( float timeStep, float endTimeMSec ) { idPhysics_AF::PrimaryFactor ================ */ -void idPhysics_AF::PrimaryFactor( void ) { +void idPhysics_AF::PrimaryFactor(void) { int i; - for ( i = 0; i < trees.Num(); i++ ) { + for (i = 0; i < trees.Num(); i++) { trees[i]->Factor(); } } @@ -5028,11 +5039,11 @@ void idPhysics_AF::PrimaryFactor( void ) { idPhysics_AF::PrimaryForces ================ */ -void idPhysics_AF::PrimaryForces( float timeStep ) { +void idPhysics_AF::PrimaryForces(float timeStep) { int i; - for ( i = 0; i < trees.Num(); i++ ) { - trees[i]->CalculateForces( timeStep ); + for (i = 0; i < trees.Num(); i++) { + trees[i]->CalculateForces(timeStep); } } @@ -5041,29 +5052,29 @@ void idPhysics_AF::PrimaryForces( float timeStep ) { idPhysics_AF::AuxiliaryForces ================ */ -void idPhysics_AF::AuxiliaryForces( float timeStep ) { - int i, j, k, l, n, m, s, numAuxConstraints, *index, *boxIndex; - float *ptr, *j1, *j2, *dstPtr, *forcePtr; +void idPhysics_AF::AuxiliaryForces(float timeStep) { + int i, j, k, l, n, m, s, numAuxConstraints, * index, * boxIndex; + float* ptr, * j1, * j2, * dstPtr, * forcePtr; float invStep, u; - idAFBody *body; - idAFConstraint *constraint; + idAFBody* body; + idAFConstraint* constraint; idVecX tmp; idMatX jmk; idVecX rhs, w, lm, lo, hi; // get the number of one dimensional auxiliary constraints - for ( numAuxConstraints = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) { + for (numAuxConstraints = 0, i = 0; i < auxiliaryConstraints.Num(); i++) { numAuxConstraints += auxiliaryConstraints[i]->J1.GetNumRows(); } - if ( numAuxConstraints == 0 ) { + if (numAuxConstraints == 0) { return; } // allocate memory to store the body response to auxiliary constraint forces - forcePtr = (float *) _alloca16( bodies.Num() * numAuxConstraints * 8 * sizeof( float ) ); - index = (int *) _alloca16( bodies.Num() * numAuxConstraints * sizeof( int ) ); - for ( i = 0; i < bodies.Num(); i++ ) { + forcePtr = (float*)_alloca16(bodies.Num() * numAuxConstraints * 8 * sizeof(float)); + index = (int*)_alloca16(bodies.Num() * numAuxConstraints * sizeof(int)); + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; body->response = forcePtr; body->responseIndex = index; @@ -5074,87 +5085,87 @@ void idPhysics_AF::AuxiliaryForces( float timeStep ) { } // set on each body the largest index of an auxiliary constraint constraining the body - if ( af_useSymmetry.GetBool() ) { - for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) { + if (af_useSymmetry.GetBool()) { + for (k = 0, i = 0; i < auxiliaryConstraints.Num(); i++) { constraint = auxiliaryConstraints[i]; - for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) { - if ( k > constraint->body1->maxAuxiliaryIndex ) { + for (j = 0; j < constraint->J1.GetNumRows(); j++, k++) { + if (k > constraint->body1->maxAuxiliaryIndex) { constraint->body1->maxAuxiliaryIndex = k; } - if ( constraint->body2 && k > constraint->body2->maxAuxiliaryIndex ) { + if (constraint->body2 && k > constraint->body2->maxAuxiliaryIndex) { constraint->body2->maxAuxiliaryIndex = k; } } } - for ( i = 0; i < trees.Num(); i++ ) { + for (i = 0; i < trees.Num(); i++) { trees[i]->SetMaxSubTreeAuxiliaryIndex(); } } // calculate forces of primary constraints in response to the auxiliary constraint forces - for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) { + for (k = 0, i = 0; i < auxiliaryConstraints.Num(); i++) { constraint = auxiliaryConstraints[i]; - for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) { + for (j = 0; j < constraint->J1.GetNumRows(); j++, k++) { // calculate body forces in the tree in response to the constraint force - constraint->body1->tree->Response( constraint, j, k ); + constraint->body1->tree->Response(constraint, j, k); // if there is a second body which is part of a different tree - if ( constraint->body2 && constraint->body2->tree != constraint->body1->tree ) { + if (constraint->body2 && constraint->body2->tree != constraint->body1->tree) { // calculate body forces in the second tree in response to the constraint force - constraint->body2->tree->Response( constraint, j, k ); + constraint->body2->tree->Response(constraint, j, k); } } } // NOTE: the rows are 16 byte padded - jmk.SetData( numAuxConstraints, ((numAuxConstraints+3)&~3), MATX_ALLOCA( numAuxConstraints * ((numAuxConstraints+3)&~3) ) ); - tmp.SetData( 6, VECX_ALLOCA( 6 ) ); + jmk.SetData(numAuxConstraints, ((numAuxConstraints + 3) & ~3), MATX_ALLOCA(numAuxConstraints * ((numAuxConstraints + 3) & ~3))); + tmp.SetData(6, VECX_ALLOCA(6)); // create constraint matrix for auxiliary constraints using a mass matrix adjusted for the primary constraints - for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) { + for (k = 0, i = 0; i < auxiliaryConstraints.Num(); i++) { constraint = auxiliaryConstraints[i]; - for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) { - constraint->body1->InverseWorldSpatialInertiaMultiply( tmp, constraint->J1[j] ); + for (j = 0; j < constraint->J1.GetNumRows(); j++, k++) { + constraint->body1->InverseWorldSpatialInertiaMultiply(tmp, constraint->J1[j]); j1 = tmp.ToFloatPtr(); ptr = constraint->body1->response; index = constraint->body1->responseIndex; dstPtr = jmk[k]; s = af_useSymmetry.GetBool() ? k + 1 : numAuxConstraints; - for ( l = n = 0, m = index[n]; n < constraint->body1->numResponses && m < s; n++, m = index[n] ) { - while( l < m ) { + for (l = n = 0, m = index[n]; n < constraint->body1->numResponses && m < s; n++, m = index[n]) { + while (l < m) { dstPtr[l++] = 0.0f; } dstPtr[l++] = j1[0] * ptr[0] + j1[1] * ptr[1] + j1[2] * ptr[2] + - j1[3] * ptr[3] + j1[4] * ptr[4] + j1[5] * ptr[5]; + j1[3] * ptr[3] + j1[4] * ptr[4] + j1[5] * ptr[5]; ptr += 8; } - while( l < s ) { + while (l < s) { dstPtr[l++] = 0.0f; } - if ( constraint->body2 ) { - constraint->body2->InverseWorldSpatialInertiaMultiply( tmp, constraint->J2[j] ); + if (constraint->body2) { + constraint->body2->InverseWorldSpatialInertiaMultiply(tmp, constraint->J2[j]); j2 = tmp.ToFloatPtr(); ptr = constraint->body2->response; index = constraint->body2->responseIndex; - for ( n = 0, m = index[n]; n < constraint->body2->numResponses && m < s; n++, m = index[n] ) { + for (n = 0, m = index[n]; n < constraint->body2->numResponses && m < s; n++, m = index[n]) { dstPtr[m] += j2[0] * ptr[0] + j2[1] * ptr[1] + j2[2] * ptr[2] + - j2[3] * ptr[3] + j2[4] * ptr[4] + j2[5] * ptr[5]; + j2[3] * ptr[3] + j2[4] * ptr[4] + j2[5] * ptr[5]; ptr += 8; } } } } - if ( af_useSymmetry.GetBool() ) { + if (af_useSymmetry.GetBool()) { n = jmk.GetNumColumns(); - for ( i = 0; i < numAuxConstraints; i++ ) { - ptr = jmk.ToFloatPtr() + ( i + 1 ) * n + i; + for (i = 0; i < numAuxConstraints; i++) { + ptr = jmk.ToFloatPtr() + (i + 1) * n + i; dstPtr = jmk.ToFloatPtr() + i * n + i + 1; - for ( j = i+1; j < numAuxConstraints; j++ ) { + for (j = i + 1; j < numAuxConstraints; j++) { *dstPtr++ = *ptr; ptr += n; } @@ -5164,37 +5175,37 @@ void idPhysics_AF::AuxiliaryForces( float timeStep ) { invStep = 1.0f / timeStep; // calculate body acceleration - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; - body->InverseWorldSpatialInertiaMultiply( body->acceleration, body->totalForce.ToFloatPtr() ); + body->InverseWorldSpatialInertiaMultiply(body->acceleration, body->totalForce.ToFloatPtr()); body->acceleration.SubVec6(0) += body->current->spatialVelocity * invStep; } - rhs.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) ); - lo.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) ); - hi.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) ); - lm.SetData( numAuxConstraints, VECX_ALLOCA( numAuxConstraints ) ); - boxIndex = (int *) _alloca16( numAuxConstraints * sizeof( int ) ); + rhs.SetData(numAuxConstraints, VECX_ALLOCA(numAuxConstraints)); + lo.SetData(numAuxConstraints, VECX_ALLOCA(numAuxConstraints)); + hi.SetData(numAuxConstraints, VECX_ALLOCA(numAuxConstraints)); + lm.SetData(numAuxConstraints, VECX_ALLOCA(numAuxConstraints)); + boxIndex = (int*)_alloca16(numAuxConstraints * sizeof(int)); // set first index for special box constrained variables - for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) { + for (k = 0, i = 0; i < auxiliaryConstraints.Num(); i++) { auxiliaryConstraints[i]->firstIndex = k; k += auxiliaryConstraints[i]->J1.GetNumRows(); } // initialize right hand side and low and high bounds for auxiliary constraints - for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) { + for (k = 0, i = 0; i < auxiliaryConstraints.Num(); i++) { constraint = auxiliaryConstraints[i]; n = k; - for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) { + for (j = 0; j < constraint->J1.GetNumRows(); j++, k++) { j1 = constraint->J1[j]; ptr = constraint->body1->acceleration.ToFloatPtr(); rhs[k] = j1[0] * ptr[0] + j1[1] * ptr[1] + j1[2] * ptr[2] + j1[3] * ptr[3] + j1[4] * ptr[4] + j1[5] * ptr[5]; rhs[k] += constraint->c1[j] * invStep; - if ( constraint->body2 ) { + if (constraint->body2) { j2 = constraint->J2[j]; ptr = constraint->body2->acceleration.ToFloatPtr(); rhs[k] += j2[0] * ptr[0] + j2[1] * ptr[1] + j2[2] * ptr[2] + j2[3] * ptr[3] + j2[4] * ptr[4] + j2[5] * ptr[5]; @@ -5205,9 +5216,9 @@ void idPhysics_AF::AuxiliaryForces( float timeStep ) { lo[k] = constraint->lo[j]; hi[k] = constraint->hi[j]; - if ( constraint->boxIndex[j] >= 0 ) { - if ( constraint->boxConstraint->fl.isPrimary ) { - gameLocal.Error( "cannot reference primary constraints for the box index" ); + if (constraint->boxIndex[j] >= 0) { + if (constraint->boxConstraint->fl.isPrimary) { + gameLocal.Error("cannot reference primary constraints for the box index"); } boxIndex[k] = constraint->boxConstraint->firstIndex + constraint->boxIndex[j]; } @@ -5223,7 +5234,10 @@ void idPhysics_AF::AuxiliaryForces( float timeStep ) { #endif // calculate lagrange multipliers for auxiliary constraints - if ( !lcp->Solve( jmk, lm, rhs, lo, hi, boxIndex ) ) { + if (!lcp->Solve(jmk, lm, rhs, lo, hi, boxIndex)) { +#ifdef AF_TIMINGS + timer_lcp.Stop(); +#endif return; // bad monkey! } @@ -5232,10 +5246,10 @@ void idPhysics_AF::AuxiliaryForces( float timeStep ) { #endif // calculate auxiliary constraint forces - for ( k = 0, i = 0; i < auxiliaryConstraints.Num(); i++ ) { + for (k = 0, i = 0; i < auxiliaryConstraints.Num(); i++) { constraint = auxiliaryConstraints[i]; - for ( j = 0; j < constraint->J1.GetNumRows(); j++, k++ ) { + for (j = 0; j < constraint->J1.GetNumRows(); j++, k++) { constraint->lm[j] = u = lm[k]; j1 = constraint->J1[j]; @@ -5243,7 +5257,7 @@ void idPhysics_AF::AuxiliaryForces( float timeStep ) { ptr[0] += j1[0] * u; ptr[1] += j1[1] * u; ptr[2] += j1[2] * u; ptr[3] += j1[3] * u; ptr[4] += j1[4] * u; ptr[5] += j1[5] * u; - if ( constraint->body2 ) { + if (constraint->body2) { j2 = constraint->J2[j]; ptr = constraint->body2->auxForce.ToFloatPtr(); ptr[0] += j2[0] * u; ptr[1] += j2[1] * u; ptr[2] += j2[2] * u; @@ -5253,10 +5267,10 @@ void idPhysics_AF::AuxiliaryForces( float timeStep ) { } // recalculate primary constraint forces in response to auxiliary constraint forces - PrimaryForces( timeStep ); + PrimaryForces(timeStep); // clear pointers pointing to stack space so tools don't get confused - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; body->response = NULL; body->responseIndex = NULL; @@ -5268,55 +5282,55 @@ void idPhysics_AF::AuxiliaryForces( float timeStep ) { idPhysics_AF::VerifyContactConstraints ================ */ -void idPhysics_AF::VerifyContactConstraints( void ) { +void idPhysics_AF::VerifyContactConstraints(void) { #if 0 int i; float impulseNumerator, impulseDenominator; idVec3 r, velocity, normalVelocity, normal, impulse; - idAFBody *body; + idAFBody* body; - for ( i = 0; i < contactConstraints.Num(); i++ ) { + for (i = 0; i < contactConstraints.Num(); i++) { body = contactConstraints[i]->body1; - const contactInfo_t &contact = contactConstraints[i]->GetContact(); + const contactInfo_t& contact = contactConstraints[i]->GetContact(); r = contact.point - body->GetCenterOfMass(); // calculate velocity at contact point - velocity = body->GetLinearVelocity() + body->GetAngularVelocity().Cross( r ); + velocity = body->GetLinearVelocity() + body->GetAngularVelocity().Cross(r); // velocity along normal vector - normalVelocity = ( velocity * contact.normal ) * contact.normal; + normalVelocity = (velocity * contact.normal) * contact.normal; // if moving towards the surface at the contact point - if ( normalVelocity * contact.normal < 0.0f ) { + if (normalVelocity * contact.normal < 0.0f) { // calculate impulse normal = -normalVelocity; impulseNumerator = normal.Normalize(); - impulseDenominator = body->GetInverseMass() + ( ( body->GetInverseWorldInertia() * r.Cross( normal ) ).Cross( r ) * normal ); + impulseDenominator = body->GetInverseMass() + ((body->GetInverseWorldInertia() * r.Cross(normal)).Cross(r) * normal); impulse = (impulseNumerator / impulseDenominator) * normal * 1.0001f; // apply impulse - body->SetLinearVelocity( body->GetLinearVelocity() + impulse ); - body->SetAngularVelocity( body->GetAngularVelocity() + r.Cross( impulse ) ); + body->SetLinearVelocity(body->GetLinearVelocity() + impulse); + body->SetAngularVelocity(body->GetAngularVelocity() + r.Cross(impulse)); } } #else int i; - idAFBody *body; + idAFBody* body; idVec3 normal; - for ( i = 0; i < contactConstraints.Num(); i++ ) { + for (i = 0; i < contactConstraints.Num(); i++) { body = contactConstraints[i]->body1; normal = contactConstraints[i]->GetContact().normal; - if ( normal * body->next->spatialVelocity.SubVec3(0) <= 0.0f ) { + if (normal * body->next->spatialVelocity.SubVec3(0) <= 0.0f) { body->next->spatialVelocity.SubVec3(0) -= 1.0001f * (normal * body->next->spatialVelocity.SubVec3(0)) * normal; } body = contactConstraints[i]->body2; - if ( !body ) { + if (!body) { continue; } normal = -normal; - if ( normal * body->next->spatialVelocity.SubVec3(0) <= 0.0f ) { + if (normal * body->next->spatialVelocity.SubVec3(0) <= 0.0f) { body->next->spatialVelocity.SubVec3(0) -= 1.0001f * (normal * body->next->spatialVelocity.SubVec3(0)) * normal; } } @@ -5328,38 +5342,38 @@ void idPhysics_AF::VerifyContactConstraints( void ) { idPhysics_AF::Evolve ================ */ -void idPhysics_AF::Evolve( float timeStep ) { +void idPhysics_AF::Evolve(float timeStep) { int i; float angle; idVec3 vec; - idAFBody *body; + idAFBody* body; idVec6 force; idRotation rotation; float vSqr, maxLinearVelocity, maxAngularVelocity; - + maxLinearVelocity = af_maxLinearVelocity.GetFloat() / timeStep; maxAngularVelocity = af_maxAngularVelocity.GetFloat() / timeStep; - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; // calculate the spatial velocity for the next physics state - body->InverseWorldSpatialInertiaMultiply( body->acceleration, body->totalForce.ToFloatPtr() ); + body->InverseWorldSpatialInertiaMultiply(body->acceleration, body->totalForce.ToFloatPtr()); body->next->spatialVelocity = body->current->spatialVelocity + timeStep * body->acceleration.SubVec6(0); - if ( maxLinearVelocity > 0.0f ) { + if (maxLinearVelocity > 0.0f) { // cap the linear velocity vSqr = body->next->spatialVelocity.SubVec3(0).LengthSqr(); - if ( vSqr > Square( maxLinearVelocity ) ) { - body->next->spatialVelocity.SubVec3(0) *= idMath::InvSqrt( vSqr ) * maxLinearVelocity; + if (vSqr > Square(maxLinearVelocity)) { + body->next->spatialVelocity.SubVec3(0) *= idMath::InvSqrt(vSqr) * maxLinearVelocity; } } - if ( maxAngularVelocity > 0.0f ) { + if (maxAngularVelocity > 0.0f) { // cap the angular velocity vSqr = body->next->spatialVelocity.SubVec3(1).LengthSqr(); - if ( vSqr > Square( maxAngularVelocity ) ) { - body->next->spatialVelocity.SubVec3(1) *= idMath::InvSqrt( vSqr ) * maxAngularVelocity; + if (vSqr > Square(maxAngularVelocity)) { + body->next->spatialVelocity.SubVec3(1) *= idMath::InvSqrt(vSqr) * maxAngularVelocity; } } } @@ -5368,16 +5382,16 @@ void idPhysics_AF::Evolve( float timeStep ) { VerifyContactConstraints(); // calculate the position of the bodies for the next physics state - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; // translate world origin - body->next->worldOrigin = body->current->worldOrigin + timeStep * body->next->spatialVelocity.SubVec3( 0 ); + body->next->worldOrigin = body->current->worldOrigin + timeStep * body->next->spatialVelocity.SubVec3(0); // convert angular velocity to a rotation matrix - vec = body->next->spatialVelocity.SubVec3( 1 ); - angle = -timeStep * (float) RAD2DEG( vec.Normalize() ); - rotation = idRotation( vec3_origin, vec, angle ); + vec = body->next->spatialVelocity.SubVec3(1); + angle = -timeStep * (float)RAD2DEG(vec.Normalize()); + rotation = idRotation(vec3_origin, vec, angle); rotation.Normalize180(); // rotate world axis @@ -5399,20 +5413,20 @@ idPhysics_AF::CollisionImpulse this is silly as it doesn't take the AF structure into account ================ */ -bool idPhysics_AF::CollisionImpulse( float timeStep, idAFBody *body, trace_t &collision ) { +bool idPhysics_AF::CollisionImpulse(float timeStep, idAFBody* body, trace_t& collision) { idVec3 r, velocity, impulse; idMat3 inverseWorldInertiaTensor; float impulseNumerator, impulseDenominator; impactInfo_t info; - idEntity *ent; + idEntity* ent; ent = gameLocal.entities[collision.c.entityNum]; - if ( ent == self ) { + if (ent == self) { return false; } // get info from other entity involved - ent->GetImpactInfo( self, collision.c.id, collision.c.point, &info ); + ent->GetImpactInfo(self, collision.c.id, collision.c.point, &info); // collision point relative to the body center of mass r = collision.c.point - (body->current->worldOrigin + body->centerOfMass * body->current->worldAxis); // the velocity at the collision point @@ -5420,22 +5434,22 @@ bool idPhysics_AF::CollisionImpulse( float timeStep, idAFBody *body, trace_t &co // subtract velocity of other entity velocity -= info.velocity; // never stick - if ( velocity * collision.c.normal > 0.0f ) { + if (velocity * collision.c.normal > 0.0f) { velocity = collision.c.normal; } inverseWorldInertiaTensor = body->current->worldAxis.Transpose() * body->inverseInertiaTensor * body->current->worldAxis; - impulseNumerator = -( 1.0f + body->bouncyness ) * ( velocity * collision.c.normal ); - impulseDenominator = body->invMass + ( ( inverseWorldInertiaTensor * r.Cross( collision.c.normal ) ).Cross( r ) * collision.c.normal ); - if ( info.invMass ) { - impulseDenominator += info.invMass + ( ( info.invInertiaTensor * info.position.Cross( collision.c.normal ) ).Cross( info.position ) * collision.c.normal ); + impulseNumerator = -(1.0f + body->bouncyness) * (velocity * collision.c.normal); + impulseDenominator = body->invMass + ((inverseWorldInertiaTensor * r.Cross(collision.c.normal)).Cross(r) * collision.c.normal); + if (info.invMass) { + impulseDenominator += info.invMass + ((info.invInertiaTensor * info.position.Cross(collision.c.normal)).Cross(info.position) * collision.c.normal); } impulse = (impulseNumerator / impulseDenominator) * collision.c.normal; // apply impact to other entity - ent->ApplyImpulse( self, collision.c.id, collision.c.point, -impulse ); + ent->ApplyImpulse(self, collision.c.id, collision.c.point, -impulse); // callback to self to let the entity know about the impact - return self->Collide( collision, velocity ); + return self->Collide(collision, velocity); } /* @@ -5443,11 +5457,11 @@ bool idPhysics_AF::CollisionImpulse( float timeStep, idAFBody *body, trace_t &co idPhysics_AF::ApplyCollisions ================ */ -bool idPhysics_AF::ApplyCollisions( float timeStep ) { +bool idPhysics_AF::ApplyCollisions(float timeStep) { int i; - for ( i = 0; i < collisions.Num(); i++ ) { - if ( CollisionImpulse( timeStep, collisions[i].body, collisions[i].trace ) ) { + for (i = 0; i < collisions.Num(); i++) { + if (CollisionImpulse(timeStep, collisions[i].body, collisions[i].trace)) { return true; } } @@ -5459,65 +5473,70 @@ bool idPhysics_AF::ApplyCollisions( float timeStep ) { idPhysics_AF::SetupCollisionForBody ================ */ -idEntity *idPhysics_AF::SetupCollisionForBody( idAFBody *body ) const { +idEntity* idPhysics_AF::SetupCollisionForBody(idAFBody* body) const { int i; - idAFBody *b; - idEntity *passEntity; + idAFBody* b; + idEntity* passEntity; passEntity = NULL; - if ( !selfCollision || !body->fl.selfCollision || af_skipSelfCollision.GetBool() ) { + if (!selfCollision || !body->fl.selfCollision || af_skipSelfCollision.GetBool()) { - // disable all bodies - for ( i = 0; i < bodies.Num(); i++ ) { - bodies[i]->clipModel->Disable(); + // unlink all bodies so the body only collides with the world and external entities + for (i = 0; i < bodies.Num(); i++) { + bodies[i]->clipModel->Unlink(); } // don't collide with world collision model if attached to the world - for ( i = 0; i < body->constraints.Num(); i++ ) { - if ( !body->constraints[i]->fl.noCollision ) { + for (i = 0; i < body->constraints.Num(); i++) { + if (!body->constraints[i]->fl.noCollision) { continue; } // if this constraint attaches the body to the world - if ( body->constraints[i]->body2 == NULL ) { + if (body->constraints[i]->body2 == NULL) { // don't collide with the world collision model passEntity = gameLocal.world; } } - } else { + } + else { - // enable all bodies that have self collision - for ( i = 0; i < bodies.Num(); i++ ) { - if ( bodies[i]->fl.selfCollision ) { - bodies[i]->clipModel->Enable(); - } else { - bodies[i]->clipModel->Disable(); + // link only bodies that can participate in self collision + for (i = 0; i < bodies.Num(); i++) { + if (bodies[i]->fl.selfCollision) { + bodies[i]->clipModel->Link(gameLocal.clip, self, bodies[i]->clipModel->GetId(), bodies[i]->current->worldOrigin, bodies[i]->current->worldAxis); + } + else { + bodies[i]->clipModel->Unlink(); } } // don't let the body collide with itself - body->clipModel->Disable(); + body->clipModel->Unlink(); - // disable any bodies attached with constraints - for ( i = 0; i < body->constraints.Num(); i++ ) { - if ( !body->constraints[i]->fl.noCollision ) { + // unlink any bodies attached with no-collision constraints + for (i = 0; i < body->constraints.Num(); i++) { + if (!body->constraints[i]->fl.noCollision) { continue; } // if this constraint attaches the body to the world - if ( body->constraints[i]->body2 == NULL ) { + if (body->constraints[i]->body2 == NULL) { // don't collide with the world collision model passEntity = gameLocal.world; - } else { - if ( body->constraints[i]->body1 == body ) { + } + else { + if (body->constraints[i]->body1 == body) { b = body->constraints[i]->body2; - } else if ( body->constraints[i]->body2 == body ) { + } + else if (body->constraints[i]->body2 == body) { b = body->constraints[i]->body1; - } else { + } + else { continue; } // don't collide with this body - b->clipModel->Disable(); + b->clipModel->Unlink(); } } } @@ -5534,44 +5553,44 @@ idPhysics_AF::CheckForCollisions assumes all bodies are linked for collision detection and relinks all bodies after moving them ================ */ -void idPhysics_AF::CheckForCollisions( float timeStep ) { -// #define TEST_COLLISION_DETECTION +void idPhysics_AF::CheckForCollisions(float timeStep) { + // #define TEST_COLLISION_DETECTION int i, index; - idAFBody *body; + idAFBody* body; idMat3 axis; idRotation rotation; trace_t collision; - idEntity *passEntity; + idEntity* passEntity; // clear list with collisions - collisions.SetNum( 0, false ); + collisions.SetNum(0, false); - if ( !enableCollision ) { + if (!enableCollision) { return; } - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; - if ( body->clipMask != 0 ) { + if (body->clipMask != 0) { - passEntity = SetupCollisionForBody( body ); + passEntity = SetupCollisionForBody(body); #ifdef TEST_COLLISION_DETECTION bool startsolid = false; - if ( gameLocal.clip.Contents( body->current->worldOrigin, body->clipModel, - body->current->worldAxis, body->clipMask, passEntity ) ) { + if (gameLocal.clip.Contents(body->current->worldOrigin, body->clipModel, + body->current->worldAxis, body->clipMask, passEntity)) { startsolid = true; } #endif - TransposeMultiply( body->current->worldAxis, body->next->worldAxis, axis ); + TransposeMultiply(body->current->worldAxis, body->next->worldAxis, axis); rotation = axis.ToRotation(); - rotation.SetOrigin( body->current->worldOrigin ); + rotation.SetOrigin(body->current->worldOrigin); // if there was a collision - if ( gameLocal.clip.Motion( collision, body->current->worldOrigin, body->next->worldOrigin, rotation, - body->clipModel, body->current->worldAxis, body->clipMask, passEntity ) ) { + if (gameLocal.clip.Motion(collision, body->current->worldOrigin, body->next->worldOrigin, rotation, + body->clipModel, body->current->worldAxis, body->clipMask, passEntity)) { // set the next state to the state at the moment of impact body->next->worldOrigin = collision.endpos; @@ -5579,22 +5598,22 @@ void idPhysics_AF::CheckForCollisions( float timeStep ) { // add collision to the list index = collisions.Num(); - collisions.SetNum( index + 1, false ); + collisions.SetNum(index + 1, false); collisions[index].trace = collision; collisions[index].body = body; } #ifdef TEST_COLLISION_DETECTION - if ( gameLocal.clip.Contents( body->next->worldOrigin, body->clipModel, - body->next->worldAxis, body->clipMask, passEntity ) ) { - if ( !startsolid ) { + if (gameLocal.clip.Contents(body->next->worldOrigin, body->clipModel, + body->next->worldAxis, body->clipMask, passEntity)) { + if (!startsolid) { int bah = 1; } } #endif } - body->clipModel->Link( gameLocal.clip, self, body->clipModel->GetId(), body->next->worldOrigin, body->next->worldAxis ); + body->clipModel->Link(gameLocal.clip, self, body->clipModel->GetId(), body->next->worldOrigin, body->next->worldAxis); } } @@ -5603,75 +5622,76 @@ void idPhysics_AF::CheckForCollisions( float timeStep ) { idPhysics_AF::EvaluateContacts ================ */ -bool idPhysics_AF::EvaluateContacts( void ) { +bool idPhysics_AF::EvaluateContacts(void) { int i, j, k, numContacts, numBodyContacts; - idAFBody *body; + idAFBody* body; contactInfo_t contactInfo[10]; - idEntity *passEntity; - idVecX dir( 6, VECX_ALLOCA( 6 ) ); + idEntity* passEntity; + idVecX force(6, VECX_ALLOCA(6)); + idVecX dir(6, VECX_ALLOCA(6)); // evaluate bodies - EvaluateBodies( current.lastTimeStep ); + EvaluateBodies(current.lastTimeStep); // remove all existing contacts ClearContacts(); - contactBodies.SetNum( 0, false ); + contactBodies.SetNum(0, false); - if ( !enableCollision ) { + if (!enableCollision) { return false; } // find all the contacts - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; - if ( body->clipMask == 0 ) { + if (body->clipMask == 0) { continue; } - passEntity = SetupCollisionForBody( body ); + passEntity = SetupCollisionForBody(body); - body->InverseWorldSpatialInertiaMultiply( dir, body->current->externalForce.ToFloatPtr() ); - dir.SubVec6(0) = body->current->spatialVelocity + current.lastTimeStep * dir.SubVec6(0); + body->InverseWorldSpatialInertiaMultiply(force, body->current->externalForce.ToFloatPtr()); + dir.Zero(); + dir.SubVec3(0) = body->current->spatialVelocity.SubVec3(0) + current.lastTimeStep * force.SubVec3(0); dir.SubVec3(0).Normalize(); - dir.SubVec3(1).Normalize(); - numContacts = gameLocal.clip.Contacts( contactInfo, 10, body->current->worldOrigin, dir.SubVec6(0), 2.0f, //CONTACT_EPSILON, - body->clipModel, body->current->worldAxis, body->clipMask, passEntity ); + numContacts = gameLocal.clip.Contacts(contactInfo, 10, body->current->worldOrigin, dir.SubVec6(0), 2.0f, //CONTACT_EPSILON, + body->clipModel, body->current->worldAxis, body->clipMask, passEntity); #if 1 // merge nearby contacts between the same bodies // and assure there are at most three planar contacts between any pair of bodies - for ( j = 0; j < numContacts; j++ ) { + for (j = 0; j < numContacts; j++) { numBodyContacts = 0; - for ( k = 0; k < contacts.Num(); k++ ) { - if ( contacts[k].entityNum == contactInfo[j].entityNum ) { - if ( ( contacts[k].id == i && contactInfo[j].id == contactBodies[k] ) || - ( contactBodies[k] == i && contacts[k].id == contactInfo[j].id ) ) { + for (k = 0; k < contacts.Num(); k++) { + if (contacts[k].entityNum == contactInfo[j].entityNum) { + if ((contacts[k].id == i && contactInfo[j].id == contactBodies[k]) || + (contactBodies[k] == i && contacts[k].id == contactInfo[j].id)) { - if ( ( contacts[k].point - contactInfo[j].point ).LengthSqr() < Square( 2.0f ) ) { + if ((contacts[k].point - contactInfo[j].point).LengthSqr() < Square(2.0f)) { break; } - if ( idMath::Fabs( contacts[k].normal * contactInfo[j].normal ) > 0.9f ) { + if (idMath::Fabs(contacts[k].normal * contactInfo[j].normal) > 0.9f) { numBodyContacts++; } } } } - if ( k >= contacts.Num() && numBodyContacts < 3 ) { - contacts.Append( contactInfo[j] ); - contactBodies.Append( i ); + if (k >= contacts.Num() && numBodyContacts < 3) { + contacts.Append(contactInfo[j]); + contactBodies.Append(i); } } #else - for ( j = 0; j < numContacts; j++ ) { - contacts.Append( contactInfo[j] ); - contactBodies.Append( i ); + for (j = 0; j < numContacts; j++) { + contacts.Append(contactInfo[j]); + contactBodies.Append(i); } #endif @@ -5679,7 +5699,7 @@ bool idPhysics_AF::EvaluateContacts( void ) { AddContactEntitiesForContacts(); - return ( contacts.Num() != 0 ); + return (contacts.Num() != 0); } /* @@ -5687,22 +5707,34 @@ bool idPhysics_AF::EvaluateContacts( void ) { idPhysics_AF::SetupContactConstraints ================ */ -void idPhysics_AF::SetupContactConstraints( void ) { +void idPhysics_AF::SetupContactConstraints(void) { int i; - // make sure enough contact constraints are allocated - contactConstraints.AssureSizeAlloc( contacts.Num(), idListNewElement ); - contactConstraints.SetNum( contacts.Num(), false ); + // make sure enough contact constraints are allocated, and release unused ones + if (contacts.Num() > contactConstraints.Num()) { + int old = contactConstraints.Num(); + contactConstraints.SetNum(contacts.Num(), false); + for (i = old; i < contacts.Num(); i++) { + contactConstraints[i] = new idAFConstraint_Contact(); + } + } + else { + for (i = contacts.Num(); i < contactConstraints.Num(); i++) { + delete contactConstraints[i]; + contactConstraints[i] = NULL; + } + contactConstraints.SetNum(contacts.Num(), false); + } // setup contact constraints - for ( i = 0; i < contacts.Num(); i++ ) { + for (i = 0; i < contacts.Num(); i++) { // add contact constraint contactConstraints[i]->physics = this; - if ( contacts[i].entityNum == self->entityNumber ) { - contactConstraints[i]->Setup( bodies[contactBodies[i]], bodies[ contacts[i].id ], contacts[i] ); + if (contacts[i].entityNum == self->entityNumber) { + contactConstraints[i]->Setup(bodies[contactBodies[i]], bodies[contacts[i].id], contacts[i]); } else { - contactConstraints[i]->Setup( bodies[contactBodies[i]], NULL, contacts[i] ); + contactConstraints[i]->Setup(bodies[contactBodies[i]], NULL, contacts[i]); } } } @@ -5712,23 +5744,23 @@ void idPhysics_AF::SetupContactConstraints( void ) { idPhysics_AF::ApplyContactForces ================ */ -void idPhysics_AF::ApplyContactForces( void ) { +void idPhysics_AF::ApplyContactForces(void) { #if 0 int i; - idEntity *ent; + idEntity* ent; idVec3 force; - for ( i = 0; i < contactConstraints.Num(); i++ ) { - if ( contactConstraints[i]->body2 != NULL ) { + for (i = 0; i < contactConstraints.Num(); i++) { + if (contactConstraints[i]->body2 != NULL) { continue; } - const contactInfo_t &contact = contactConstraints[i]->GetContact(); + const contactInfo_t& contact = contactConstraints[i]->GetContact(); ent = gameLocal.entities[contact.entityNum]; - if ( !ent ) { + if (!ent) { continue; } force.Zero(); - ent->AddForce( self, contact.id, contact.point, force ); + ent->AddForce(self, contact.id, contact.point, force); } #endif } @@ -5738,11 +5770,11 @@ void idPhysics_AF::ApplyContactForces( void ) { idPhysics_AF::ClearExternalForce ================ */ -void idPhysics_AF::ClearExternalForce( void ) { +void idPhysics_AF::ClearExternalForce(void) { int i; - idAFBody *body; + idAFBody* body; - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; // clear external force @@ -5756,14 +5788,14 @@ void idPhysics_AF::ClearExternalForce( void ) { idPhysics_AF::AddGravity ================ */ -void idPhysics_AF::AddGravity( void ) { +void idPhysics_AF::AddGravity(void) { int i; - idAFBody *body; + idAFBody* body; - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; // add gravitational force - body->current->externalForce.SubVec3( 0 ) += body->mass * gravityVector; + body->current->externalForce.SubVec3(0) += body->mass * gravityVector; } } @@ -5772,12 +5804,12 @@ void idPhysics_AF::AddGravity( void ) { idPhysics_AF::SwapStates ================ */ -void idPhysics_AF::SwapStates( void ) { +void idPhysics_AF::SwapStates(void) { int i; - idAFBody *body; - AFBodyPState_t *swap; + idAFBody* body; + AFBodyPState_t* swap; - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; @@ -5793,13 +5825,13 @@ void idPhysics_AF::SwapStates( void ) { idPhysics_AF::UpdateClipModels ================ */ -void idPhysics_AF::UpdateClipModels( void ) { +void idPhysics_AF::UpdateClipModels(void) { int i; - idAFBody *body; + idAFBody* body; - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; - body->clipModel->Link( gameLocal.clip, self, body->clipModel->GetId(), body->current->worldOrigin, body->current->worldAxis ); + body->clipModel->Link(gameLocal.clip, self, body->clipModel->GetId(), body->current->worldOrigin, body->current->worldAxis); } } @@ -5808,7 +5840,7 @@ void idPhysics_AF::UpdateClipModels( void ) { idPhysics_AF::SetSuspendSpeed ================ */ -void idPhysics_AF::SetSuspendSpeed( const idVec2 &velocity, const idVec2 &acceleration ) { +void idPhysics_AF::SetSuspendSpeed(const idVec2& velocity, const idVec2& acceleration) { this->suspendVelocity = velocity; this->suspendAcceleration = acceleration; } @@ -5818,7 +5850,7 @@ void idPhysics_AF::SetSuspendSpeed( const idVec2 &velocity, const idVec2 &accele idPhysics_AF::SetSuspendTime ================ */ -void idPhysics_AF::SetSuspendTime( const float minTime, const float maxTime ) { +void idPhysics_AF::SetSuspendTime(const float minTime, const float maxTime) { this->minMoveTime = minTime; this->maxMoveTime = maxTime; } @@ -5828,7 +5860,7 @@ void idPhysics_AF::SetSuspendTime( const float minTime, const float maxTime ) { idPhysics_AF::SetSuspendTolerance ================ */ -void idPhysics_AF::SetSuspendTolerance( const float noMoveTime, const float noMoveTranslation, const float noMoveRotation ) { +void idPhysics_AF::SetSuspendTolerance(const float noMoveTime, const float noMoveTranslation, const float noMoveRotation) { this->noMoveTime = noMoveTime; this->noMoveTranslation = noMoveTranslation; this->noMoveRotation = noMoveRotation; @@ -5839,7 +5871,7 @@ void idPhysics_AF::SetSuspendTolerance( const float noMoveTime, const float noMo idPhysics_AF::SetTimeScaleRamp ================ */ -void idPhysics_AF::SetTimeScaleRamp( const float start, const float end ) { +void idPhysics_AF::SetTimeScaleRamp(const float start, const float end) { timeScaleRampStart = start; timeScaleRampEnd = end; } @@ -5849,7 +5881,7 @@ void idPhysics_AF::SetTimeScaleRamp( const float start, const float end ) { idPhysics_AF::SetJointFrictionDent ================ */ -void idPhysics_AF::SetJointFrictionDent( const float dent, const float start, const float end ) { +void idPhysics_AF::SetJointFrictionDent(const float dent, const float start, const float end) { jointFrictionDent = dent; jointFrictionDentStart = start; jointFrictionDentEnd = end; @@ -5860,12 +5892,14 @@ void idPhysics_AF::SetJointFrictionDent( const float dent, const float start, co idPhysics_AF::GetJointFrictionScale ================ */ -float idPhysics_AF::GetJointFrictionScale( void ) const { - if ( jointFrictionDentScale > 0.0f ) { +float idPhysics_AF::GetJointFrictionScale(void) const { + if (jointFrictionDentScale > 0.0f) { return jointFrictionDentScale; - } else if ( jointFrictionScale > 0.0f ) { + } + else if (jointFrictionScale > 0.0f) { return jointFrictionScale; - } else if ( af_jointFrictionScale.GetFloat() > 0.0f ) { + } + else if (af_jointFrictionScale.GetFloat() > 0.0f) { return af_jointFrictionScale.GetFloat(); } return 1.0f; @@ -5876,7 +5910,7 @@ float idPhysics_AF::GetJointFrictionScale( void ) const { idPhysics_AF::SetContactFrictionDent ================ */ -void idPhysics_AF::SetContactFrictionDent( const float dent, const float start, const float end ) { +void idPhysics_AF::SetContactFrictionDent(const float dent, const float start, const float end) { contactFrictionDent = dent; contactFrictionDentStart = start; contactFrictionDentEnd = end; @@ -5887,12 +5921,14 @@ void idPhysics_AF::SetContactFrictionDent( const float dent, const float start, idPhysics_AF::GetContactFrictionScale ================ */ -float idPhysics_AF::GetContactFrictionScale( void ) const { - if ( contactFrictionDentScale > 0.0f ) { +float idPhysics_AF::GetContactFrictionScale(void) const { + if (contactFrictionDentScale > 0.0f) { return contactFrictionDentScale; - } else if ( contactFrictionScale > 0.0f ) { + } + else if (contactFrictionScale > 0.0f) { return contactFrictionScale; - } else if ( af_contactFrictionScale.GetFloat() > 0.0f ) { + } + else if (af_contactFrictionScale.GetFloat() > 0.0f) { return af_contactFrictionScale.GetFloat(); } return 1.0f; @@ -5903,75 +5939,76 @@ float idPhysics_AF::GetContactFrictionScale( void ) const { idPhysics_AF::TestIfAtRest ================ */ -bool idPhysics_AF::TestIfAtRest( float timeStep ) { +bool idPhysics_AF::TestIfAtRest(float timeStep) { int i; float translationSqr, maxTranslationSqr, rotation, maxRotation; - idAFBody *body; + idAFBody* body; - if ( current.atRest >= 0 ) { + if (current.atRest >= 0) { return true; } current.activateTime += timeStep; // if the simulation should never be suspended before a certaint amount of time passed - if ( minMoveTime > 0.0f && current.activateTime < minMoveTime ) { + if (minMoveTime > 0.0f && current.activateTime < minMoveTime) { return false; } // if the simulation should always be suspended after a certain amount time passed - if ( maxMoveTime > 0.0f && current.activateTime > maxMoveTime ) { + if (maxMoveTime > 0.0f && current.activateTime > maxMoveTime) { return true; } // test if all bodies hardly moved over a period of time - if ( current.noMoveTime == 0.0f ) { - for ( i = 0; i < bodies.Num(); i++ ) { + if (current.noMoveTime == 0.0f) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; body->atRestOrigin = body->current->worldOrigin; body->atRestAxis = body->current->worldAxis; } current.noMoveTime += timeStep; } - else if ( current.noMoveTime > noMoveTime ) { + else if (current.noMoveTime > noMoveTime) { current.noMoveTime = 0.0f; maxTranslationSqr = 0.0f; maxRotation = 0.0f; - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; - translationSqr = ( body->current->worldOrigin - body->atRestOrigin ).LengthSqr(); - if ( translationSqr > maxTranslationSqr ) { + translationSqr = (body->current->worldOrigin - body->atRestOrigin).LengthSqr(); + if (translationSqr > maxTranslationSqr) { maxTranslationSqr = translationSqr; } - rotation = ( body->atRestAxis.Transpose() * body->current->worldAxis ).ToRotation().GetAngle(); - if ( rotation > maxRotation ) { + rotation = (body->atRestAxis.Transpose() * body->current->worldAxis).ToRotation().GetAngle(); + if (rotation > maxRotation) { maxRotation = rotation; } } - if ( maxTranslationSqr < Square( noMoveTranslation ) && maxRotation < noMoveRotation ) { + if (maxTranslationSqr < Square(noMoveTranslation) && maxRotation < noMoveRotation) { // hardly moved over a period of time so the articulated figure may come to rest return true; } - } else { + } + else { current.noMoveTime += timeStep; } // test if the velocity or acceleration of any body is still too large to come to rest - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; - if ( body->current->spatialVelocity.SubVec3(0).LengthSqr() > Square( suspendVelocity[0] ) ) { + if (body->current->spatialVelocity.SubVec3(0).LengthSqr() > Square(suspendVelocity[0])) { return false; } - if ( body->current->spatialVelocity.SubVec3(1).LengthSqr() > Square( suspendVelocity[1] ) ) { + if (body->current->spatialVelocity.SubVec3(1).LengthSqr() > Square(suspendVelocity[1])) { return false; } - if ( body->acceleration.SubVec3(0).LengthSqr() > Square( suspendAcceleration[0] ) ) { + if (body->acceleration.SubVec3(0).LengthSqr() > Square(suspendAcceleration[0])) { return false; } - if ( body->acceleration.SubVec3(1).LengthSqr() > Square( suspendAcceleration[1] ) ) { + if (body->acceleration.SubVec3(1).LengthSqr() > Square(suspendAcceleration[1])) { return false; } } @@ -5985,17 +6022,17 @@ bool idPhysics_AF::TestIfAtRest( float timeStep ) { idPhysics_AF::Rest ================ */ -void idPhysics_AF::Rest( void ) { +void idPhysics_AF::Rest(void) { int i; current.atRest = gameLocal.time; - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { bodies[i]->current->spatialVelocity.Zero(); bodies[i]->current->externalForce.Zero(); } - self->BecomeInactive( TH_PHYSICS ); + self->BecomeInactive(TH_PHYSICS); } /* @@ -6003,9 +6040,9 @@ void idPhysics_AF::Rest( void ) { idPhysics_AF::Activate ================ */ -void idPhysics_AF::Activate( void ) { +void idPhysics_AF::Activate(void) { // if the articulated figure was at rest - if ( current.atRest >= 0 ) { + if (current.atRest >= 0) { // normally gravity is added at the end of a simulation frame // if the figure was at rest add gravity here so it is applied this simulation frame AddGravity(); @@ -6014,7 +6051,7 @@ void idPhysics_AF::Activate( void ) { } current.atRest = -1; current.noMoveTime = 0.0f; - self->BecomeActive( TH_PHYSICS ); + self->BecomeActive(TH_PHYSICS); } /* @@ -6024,7 +6061,7 @@ idPhysics_AF::PutToRest put to rest untill something collides with this physics object ================ */ -void idPhysics_AF::PutToRest( void ) { +void idPhysics_AF::PutToRest(void) { Rest(); } @@ -6033,7 +6070,7 @@ void idPhysics_AF::PutToRest( void ) { idPhysics_AF::EnableImpact ================ */ -void idPhysics_AF::EnableImpact( void ) { +void idPhysics_AF::EnableImpact(void) { noImpact = false; } @@ -6042,7 +6079,7 @@ void idPhysics_AF::EnableImpact( void ) { idPhysics_AF::DisableImpact ================ */ -void idPhysics_AF::DisableImpact( void ) { +void idPhysics_AF::DisableImpact(void) { noImpact = true; } @@ -6051,11 +6088,11 @@ void idPhysics_AF::DisableImpact( void ) { idPhysics_AF::AddPushVelocity ================ */ -void idPhysics_AF::AddPushVelocity( const idVec6 &pushVelocity ) { +void idPhysics_AF::AddPushVelocity(const idVec6& pushVelocity) { int i; - if ( pushVelocity != vec6_origin ) { - for ( i = 0; i < bodies.Num(); i++ ) { + if (pushVelocity != vec6_origin) { + for (i = 0; i < bodies.Num(); i++) { bodies[i]->current->spatialVelocity += pushVelocity; } } @@ -6066,7 +6103,8 @@ void idPhysics_AF::AddPushVelocity( const idVec6 &pushVelocity ) { idPhysics_AF::SetClipModel ================ */ -void idPhysics_AF::SetClipModel( idClipModel *model, float density, int id, bool freeOld ) { +void idPhysics_AF::SetClipModel(idClipModel* model, float density, int id, bool freeOld) { + assert(false); } /* @@ -6074,8 +6112,8 @@ void idPhysics_AF::SetClipModel( idClipModel *model, float density, int id, bool idPhysics_AF::GetClipModel ================ */ -idClipModel *idPhysics_AF::GetClipModel( int id ) const { - if ( id >= 0 && id < bodies.Num() ) { +idClipModel* idPhysics_AF::GetClipModel(int id) const { + if (id >= 0 && id < bodies.Num()) { return bodies[id]->GetClipModel(); } return NULL; @@ -6086,7 +6124,7 @@ idClipModel *idPhysics_AF::GetClipModel( int id ) const { idPhysics_AF::GetNumClipModels ================ */ -int idPhysics_AF::GetNumClipModels( void ) const { +int idPhysics_AF::GetNumClipModels(void) const { return bodies.Num(); } @@ -6095,8 +6133,8 @@ int idPhysics_AF::GetNumClipModels( void ) const { idPhysics_AF::SetMass ================ */ -void idPhysics_AF::SetMass( float mass, int id ) { - if ( id >= 0 && id < bodies.Num() ) { +void idPhysics_AF::SetMass(float mass, int id) { + if (id >= 0 && id < bodies.Num()) { } else { forceTotalMass = mass; @@ -6109,8 +6147,8 @@ void idPhysics_AF::SetMass( float mass, int id ) { idPhysics_AF::GetMass ================ */ -float idPhysics_AF::GetMass( int id ) const { - if ( id >= 0 && id < bodies.Num() ) { +float idPhysics_AF::GetMass(int id) const { + if (id >= 0 && id < bodies.Num()) { return bodies[id]->mass; } return totalMass; @@ -6121,15 +6159,15 @@ float idPhysics_AF::GetMass( int id ) const { idPhysics_AF::SetContents ================ */ -void idPhysics_AF::SetContents( int contents, int id ) { +void idPhysics_AF::SetContents(int contents, int id) { int i; - if ( id >= 0 && id < bodies.Num() ) { - bodies[id]->GetClipModel()->SetContents( contents ); + if (id >= 0 && id < bodies.Num()) { + bodies[id]->GetClipModel()->SetContents(contents); } else { - for ( i = 0; i < bodies.Num(); i++ ) { - bodies[i]->GetClipModel()->SetContents( contents ); + for (i = 0; i < bodies.Num(); i++) { + bodies[i]->GetClipModel()->SetContents(contents); } } } @@ -6139,15 +6177,15 @@ void idPhysics_AF::SetContents( int contents, int id ) { idPhysics_AF::GetContents ================ */ -int idPhysics_AF::GetContents( int id ) const { +int idPhysics_AF::GetContents(int id) const { int i, contents; - if ( id >= 0 && id < bodies.Num() ) { + if (id >= 0 && id < bodies.Num()) { return bodies[id]->GetClipModel()->GetContents(); } else { contents = 0; - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { contents |= bodies[i]->GetClipModel()->GetContents(); } return contents; @@ -6159,24 +6197,24 @@ int idPhysics_AF::GetContents( int id ) const { idPhysics_AF::GetBounds ================ */ -const idBounds &idPhysics_AF::GetBounds( int id ) const { +const idBounds& idPhysics_AF::GetBounds(int id) const { int i; static idBounds relBounds; - if ( id >= 0 && id < bodies.Num() ) { + if (id >= 0 && id < bodies.Num()) { return bodies[id]->GetClipModel()->GetBounds(); } - else if ( !bodies.Num() ) { + else if (!bodies.Num()) { relBounds.Zero(); return relBounds; } else { relBounds = bodies[0]->GetClipModel()->GetBounds(); - for ( i = 1; i < bodies.Num(); i++ ) { + for (i = 1; i < bodies.Num(); i++) { idBounds bounds; - idVec3 origin = ( bodies[i]->GetWorldOrigin() - bodies[0]->GetWorldOrigin() ) * bodies[0]->GetWorldAxis().Transpose(); + idVec3 origin = (bodies[i]->GetWorldOrigin() - bodies[0]->GetWorldOrigin()) * bodies[0]->GetWorldAxis().Transpose(); idMat3 axis = bodies[i]->GetWorldAxis() * bodies[0]->GetWorldAxis().Transpose(); - bounds.FromTransformedBounds( bodies[i]->GetClipModel()->GetBounds(), origin, axis ); + bounds.FromTransformedBounds(bodies[i]->GetClipModel()->GetBounds(), origin, axis); relBounds += bounds; } return relBounds; @@ -6188,20 +6226,20 @@ const idBounds &idPhysics_AF::GetBounds( int id ) const { idPhysics_AF::GetAbsBounds ================ */ -const idBounds &idPhysics_AF::GetAbsBounds( int id ) const { +const idBounds& idPhysics_AF::GetAbsBounds(int id) const { int i; static idBounds absBounds; - if ( id >= 0 && id < bodies.Num() ) { + if (id >= 0 && id < bodies.Num()) { return bodies[id]->GetClipModel()->GetAbsBounds(); } - else if ( !bodies.Num() ) { + else if (!bodies.Num()) { absBounds.Zero(); return absBounds; } else { absBounds = bodies[0]->GetClipModel()->GetAbsBounds(); - for ( i = 1; i < bodies.Num(); i++ ) { + for (i = 1; i < bodies.Num(); i++) { absBounds += bodies[i]->GetClipModel()->GetAbsBounds(); } return absBounds; @@ -6213,32 +6251,34 @@ const idBounds &idPhysics_AF::GetAbsBounds( int id ) const { idPhysics_AF::Evaluate ================ */ -bool idPhysics_AF::Evaluate( int timeStepMSec, int endTimeMSec ) { +bool idPhysics_AF::Evaluate(int timeStepMSec, int endTimeMSec) { float timeStep; - if ( timeScaleRampStart < MS2SEC( endTimeMSec ) && timeScaleRampEnd > MS2SEC( endTimeMSec ) ) { - timeStep = MS2SEC( timeStepMSec ) * ( MS2SEC( endTimeMSec ) - timeScaleRampStart ) / ( timeScaleRampEnd - timeScaleRampStart ); - } else if ( af_timeScale.GetFloat() != 1.0f ) { - timeStep = MS2SEC( timeStepMSec ) * af_timeScale.GetFloat(); - } else { - timeStep = MS2SEC( timeStepMSec ) * timeScale; + if (timeScaleRampStart < MS2SEC(endTimeMSec) && timeScaleRampEnd > MS2SEC(endTimeMSec)) { + timeStep = MS2SEC(timeStepMSec) * (MS2SEC(endTimeMSec) - timeScaleRampStart) / (timeScaleRampEnd - timeScaleRampStart); + } + else if (af_timeScale.GetFloat() != 1.0f) { + timeStep = MS2SEC(timeStepMSec) * af_timeScale.GetFloat(); + } + else { + timeStep = MS2SEC(timeStepMSec) * timeScale; } current.lastTimeStep = timeStep; // if the articulated figure changed - if ( changedAF || ( linearTime != af_useLinearTime.GetBool() ) ) { + if (changedAF || (linearTime != af_useLinearTime.GetBool())) { BuildTrees(); changedAF = false; linearTime = af_useLinearTime.GetBool(); } // get the new master position - if ( masterBody ) { + if (masterBody) { idVec3 masterOrigin; idMat3 masterAxis; - self->GetMasterPosition( masterOrigin, masterAxis ); - if ( current.atRest >= 0 && ( masterBody->current->worldOrigin != masterOrigin || masterBody->current->worldAxis != masterAxis ) ) { + self->GetMasterPosition(masterOrigin, masterAxis); + if (current.atRest >= 0 && (masterBody->current->worldOrigin != masterOrigin || masterBody->current->worldAxis != masterAxis)) { Activate(); } masterBody->current->worldOrigin = masterOrigin; @@ -6246,13 +6286,13 @@ bool idPhysics_AF::Evaluate( int timeStepMSec, int endTimeMSec ) { } // if the simulation is suspended because the figure is at rest - if ( current.atRest >= 0 || timeStep <= 0.0f ) { + if (current.atRest >= 0 || timeStep <= 0.0f) { DebugDraw(); return false; } // move the af velocity into the frame of a pusher - AddPushVelocity( -current.pushVelocity ); + AddPushVelocity(-current.pushVelocity); #ifdef AF_TIMINGS timer_total.Start(); @@ -6273,20 +6313,20 @@ bool idPhysics_AF::Evaluate( int timeStepMSec, int endTimeMSec ) { #endif // evaluate constraint equations - EvaluateConstraints( timeStep ); + EvaluateConstraints(timeStep); // apply friction - ApplyFriction( timeStep, endTimeMSec ); + ApplyFriction(timeStep, endTimeMSec); // add frame constraints AddFrameConstraints(); #ifdef AF_TIMINGS int i, numPrimary = 0, numAuxiliary = 0; - for ( i = 0; i < primaryConstraints.Num(); i++ ) { + for (i = 0; i < primaryConstraints.Num(); i++) { numPrimary += primaryConstraints[i]->J1.GetNumRows(); } - for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) { + for (i = 0; i < auxiliaryConstraints.Num(); i++) { numAuxiliary += auxiliaryConstraints[i]->J1.GetNumRows(); } timer_pc.Start(); @@ -6296,7 +6336,7 @@ bool idPhysics_AF::Evaluate( int timeStepMSec, int endTimeMSec ) { PrimaryFactor(); // calculate forces on bodies after applying primary constraints - PrimaryForces( timeStep ); + PrimaryForces(timeStep); #ifdef AF_TIMINGS timer_pc.Stop(); @@ -6304,14 +6344,14 @@ bool idPhysics_AF::Evaluate( int timeStepMSec, int endTimeMSec ) { #endif // calculate and apply auxiliary constraint forces - AuxiliaryForces( timeStep ); + AuxiliaryForces(timeStep); #ifdef AF_TIMINGS timer_ac.Stop(); #endif // evolve current state to next state - Evolve( timeStep ); + Evolve(timeStep); // debug graphics DebugDraw(); @@ -6330,7 +6370,7 @@ bool idPhysics_AF::Evaluate( int timeStepMSec, int endTimeMSec ) { #endif // check for collisions between current and next state - CheckForCollisions( timeStep ); + CheckForCollisions(timeStep); #ifdef AF_TIMINGS timer_collision.Stop(); @@ -6339,21 +6379,22 @@ bool idPhysics_AF::Evaluate( int timeStepMSec, int endTimeMSec ) { // swap the current and next state SwapStates(); - // make sure all clip models are disabled in case they were enabled for self collision - if ( selfCollision && !af_skipSelfCollision.GetBool() ) { - DisableClip(); + // make sure all clip models are unlinked in case they were linked for self collision + if (selfCollision && !af_skipSelfCollision.GetBool()) { + UnlinkClip(); } // apply collision impulses - if ( ApplyCollisions( timeStep ) ) { + if (ApplyCollisions(timeStep)) { current.atRest = gameLocal.time; comeToRest = true; } // test if the simulation can be suspended because the whole figure is at rest - if ( comeToRest && TestIfAtRest( timeStep ) ) { + if (comeToRest && TestIfAtRest(timeStep)) { Rest(); - } else { + } + else { ActivateContactEntities(); } @@ -6361,39 +6402,39 @@ bool idPhysics_AF::Evaluate( int timeStepMSec, int endTimeMSec ) { AddGravity(); // move the af velocity back into the world frame - AddPushVelocity( current.pushVelocity ); + AddPushVelocity(current.pushVelocity); current.pushVelocity.Zero(); - if ( IsOutsideWorld() ) { - gameLocal.Warning( "articulated figure moved outside world bounds for entity '%s' type '%s' at (%s)", - self->name.c_str(), self->GetType()->classname, bodies[0]->current->worldOrigin.ToString(0) ); + if (IsOutsideWorld()) { + gameLocal.Warning("articulated figure moved outside world bounds for entity '%s' type '%s' at (%s)", + self->name.c_str(), self->GetType()->classname, bodies[0]->current->worldOrigin.ToString(0)); Rest(); } #ifdef AF_TIMINGS timer_total.Stop(); - if ( af_showTimings.GetInteger() == 1 ) { - gameLocal.Printf( "%12s: t %1.4f pc %2d, %1.4f ac %2d %1.4f lcp %1.4f cd %1.4f\n", - self->name.c_str(), - timer_total.Milliseconds(), - numPrimary, timer_pc.Milliseconds(), - numAuxiliary, timer_ac.Milliseconds() - timer_lcp.Milliseconds(), - timer_lcp.Milliseconds(), timer_collision.Milliseconds() ); + if (af_showTimings.GetInteger() == 1) { + gameLocal.Printf("%12s: t %1.4f pc %2d, %1.4f ac %2d %1.4f lcp %1.4f cd %1.4f\n", + self->name.c_str(), + timer_total.Milliseconds(), + numPrimary, timer_pc.Milliseconds(), + numAuxiliary, timer_ac.Milliseconds() - timer_lcp.Milliseconds(), + timer_lcp.Milliseconds(), timer_collision.Milliseconds()); } - else if ( af_showTimings.GetInteger() == 2 ) { + else if (af_showTimings.GetInteger() == 2) { numArticulatedFigures++; - if ( endTimeMSec > lastTimerReset ) { - gameLocal.Printf( "af %d: t %1.4f pc %2d, %1.4f ac %2d %1.4f lcp %1.4f cd %1.4f\n", - numArticulatedFigures, - timer_total.Milliseconds(), - numPrimary, timer_pc.Milliseconds(), - numAuxiliary, timer_ac.Milliseconds() - timer_lcp.Milliseconds(), - timer_lcp.Milliseconds(), timer_collision.Milliseconds() ); + if (endTimeMSec > lastTimerReset) { + gameLocal.Printf("af %d: t %1.4f pc %2d, %1.4f ac %2d %1.4f lcp %1.4f cd %1.4f\n", + numArticulatedFigures, + timer_total.Milliseconds(), + numPrimary, timer_pc.Milliseconds(), + numAuxiliary, timer_ac.Milliseconds() - timer_lcp.Milliseconds(), + timer_lcp.Milliseconds(), timer_collision.Milliseconds()); } } - if ( endTimeMSec > lastTimerReset ) { + if (endTimeMSec > lastTimerReset) { lastTimerReset = endTimeMSec; numArticulatedFigures = 0; timer_total.Clear(); @@ -6412,7 +6453,7 @@ bool idPhysics_AF::Evaluate( int timeStepMSec, int endTimeMSec ) { idPhysics_AF::UpdateTime ================ */ -void idPhysics_AF::UpdateTime( int endTimeMSec ) { +void idPhysics_AF::UpdateTime(int endTimeMSec) { } /* @@ -6420,7 +6461,7 @@ void idPhysics_AF::UpdateTime( int endTimeMSec ) { idPhysics_AF::GetTime ================ */ -int idPhysics_AF::GetTime( void ) const { +int idPhysics_AF::GetTime(void) const { return gameLocal.time; } @@ -6429,19 +6470,19 @@ int idPhysics_AF::GetTime( void ) const { DrawTraceModelSilhouette ================ */ -void DrawTraceModelSilhouette( const idVec3 &projectionOrigin, const idClipModel *clipModel ) { +void DrawTraceModelSilhouette(const idVec3& projectionOrigin, const idClipModel* clipModel) { int i, numSilEdges; int silEdges[MAX_TRACEMODEL_EDGES]; idVec3 v1, v2; - const idTraceModel *trm = clipModel->GetTraceModel(); - const idVec3 &origin = clipModel->GetOrigin(); - const idMat3 &axis = clipModel->GetAxis(); + const idTraceModel* trm = clipModel->GetTraceModel(); + const idVec3& origin = clipModel->GetOrigin(); + const idMat3& axis = clipModel->GetAxis(); - numSilEdges = trm->GetProjectionSilhouetteEdges( ( projectionOrigin - origin ) * axis.Transpose(), silEdges ); - for ( i = 0; i < numSilEdges; i++ ) { - v1 = trm->verts[ trm->edges[ abs(silEdges[i]) ].v[ INTSIGNBITSET( silEdges[i] ) ] ]; - v2 = trm->verts[ trm->edges[ abs(silEdges[i]) ].v[ INTSIGNBITNOTSET( silEdges[i] ) ] ]; - gameRenderWorld->DebugArrow( colorRed, origin + v1 * axis, origin + v2 * axis, 1 ); + numSilEdges = trm->GetProjectionSilhouetteEdges((projectionOrigin - origin) * axis.Transpose(), silEdges); + for (i = 0; i < numSilEdges; i++) { + v1 = trm->verts[trm->edges[abs(silEdges[i])].v[INTSIGNBITSET(silEdges[i])]]; + v2 = trm->verts[trm->edges[abs(silEdges[i])].v[INTSIGNBITNOTSET(silEdges[i])]]; + gameRenderWorld->DebugArrow(colorRed, origin + v1 * axis, origin + v2 * axis, 1); } } @@ -6450,131 +6491,131 @@ void DrawTraceModelSilhouette( const idVec3 &projectionOrigin, const idClipModel idPhysics_AF::DebugDraw ================ */ -void idPhysics_AF::DebugDraw( void ) { +void idPhysics_AF::DebugDraw(void) { int i; - idAFBody *body, *highlightBody = NULL, *constrainedBody1 = NULL, *constrainedBody2 = NULL; - idAFConstraint *constraint; + idAFBody* body, * highlightBody = NULL, * constrainedBody1 = NULL, * constrainedBody2 = NULL; + idAFConstraint* constraint; idVec3 center; idMat3 axis; - if ( af_highlightConstraint.GetString()[0] ) { - constraint = GetConstraint( af_highlightConstraint.GetString() ); - if ( constraint ) { - constraint->GetCenter( center ); + if (af_highlightConstraint.GetString()[0]) { + constraint = GetConstraint(af_highlightConstraint.GetString()); + if (constraint) { + constraint->GetCenter(center); axis = gameLocal.GetLocalPlayer()->viewAngles.ToMat3(); - gameRenderWorld->DebugCone( colorYellow, center, (axis[2] - axis[1]) * 4.0f, 0.0f, 1.0f, 0 ); + gameRenderWorld->DebugCone(colorYellow, center, (axis[2] - axis[1]) * 4.0f, 0.0f, 1.0f, 0); - if ( af_showConstrainedBodies.GetBool() ) { - cvarSystem->SetCVarString( "cm_drawColor", colorCyan.ToString( 0 ) ); + if (af_showConstrainedBodies.GetBool()) { + cvarSystem->SetCVarString("cm_drawColor", colorCyan.ToString(0)); constrainedBody1 = constraint->body1; - if ( constrainedBody1 ) { - collisionModelManager->DrawModel( constrainedBody1->clipModel->Handle(), constrainedBody1->clipModel->GetOrigin(), - constrainedBody1->clipModel->GetAxis(), vec3_origin, 0.0f ); + if (constrainedBody1) { + collisionModelManager->DrawModel(constrainedBody1->clipModel->Handle(), constrainedBody1->clipModel->GetOrigin(), + constrainedBody1->clipModel->GetAxis(), vec3_origin, 0.0f); } - cvarSystem->SetCVarString( "cm_drawColor", colorBlue.ToString( 0 ) ); + cvarSystem->SetCVarString("cm_drawColor", colorBlue.ToString(0)); constrainedBody2 = constraint->body2; - if ( constrainedBody2 ) { - collisionModelManager->DrawModel( constrainedBody2->clipModel->Handle(), constrainedBody2->clipModel->GetOrigin(), - constrainedBody2->clipModel->GetAxis(), vec3_origin, 0.0f ); + if (constrainedBody2) { + collisionModelManager->DrawModel(constrainedBody2->clipModel->Handle(), constrainedBody2->clipModel->GetOrigin(), + constrainedBody2->clipModel->GetAxis(), vec3_origin, 0.0f); } - cvarSystem->SetCVarString( "cm_drawColor", colorRed.ToString( 0 ) ); + cvarSystem->SetCVarString("cm_drawColor", colorRed.ToString(0)); } } } - if ( af_highlightBody.GetString()[0] ) { - highlightBody = GetBody( af_highlightBody.GetString() ); - if ( highlightBody ) { - cvarSystem->SetCVarString( "cm_drawColor", colorYellow.ToString( 0 ) ); - collisionModelManager->DrawModel( highlightBody->clipModel->Handle(), highlightBody->clipModel->GetOrigin(), - highlightBody->clipModel->GetAxis(), vec3_origin, 0.0f ); - cvarSystem->SetCVarString( "cm_drawColor", colorRed.ToString( 0 ) ); + if (af_highlightBody.GetString()[0]) { + highlightBody = GetBody(af_highlightBody.GetString()); + if (highlightBody) { + cvarSystem->SetCVarString("cm_drawColor", colorYellow.ToString(0)); + collisionModelManager->DrawModel(highlightBody->clipModel->Handle(), highlightBody->clipModel->GetOrigin(), + highlightBody->clipModel->GetAxis(), vec3_origin, 0.0f); + cvarSystem->SetCVarString("cm_drawColor", colorRed.ToString(0)); } } - if ( af_showBodies.GetBool() ) { - for ( i = 0; i < bodies.Num(); i++ ) { + if (af_showBodies.GetBool()) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; - if ( body == constrainedBody1 || body == constrainedBody2 ) { + if (body == constrainedBody1 || body == constrainedBody2) { continue; } - if ( body == highlightBody ) { + if (body == highlightBody) { continue; } - collisionModelManager->DrawModel( body->clipModel->Handle(), body->clipModel->GetOrigin(), - body->clipModel->GetAxis(), vec3_origin, 0.0f ); + collisionModelManager->DrawModel(body->clipModel->Handle(), body->clipModel->GetOrigin(), + body->clipModel->GetAxis(), vec3_origin, 0.0f); //DrawTraceModelSilhouette( gameLocal.GetLocalPlayer()->GetEyePosition(), body->clipModel ); } } - if ( af_showBodyNames.GetBool() ) { - for ( i = 0; i < bodies.Num(); i++ ) { + if (af_showBodyNames.GetBool()) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; - gameRenderWorld->DrawText( body->GetName().c_str(), body->GetWorldOrigin(), 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 ); + gameRenderWorld->DrawText(body->GetName().c_str(), body->GetWorldOrigin(), 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1); } } - if ( af_showMass.GetBool() ) { - for ( i = 0; i < bodies.Num(); i++ ) { + if (af_showMass.GetBool()) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; - gameRenderWorld->DrawText( va( "\n%1.2f", 1.0f / body->GetInverseMass() ), body->GetWorldOrigin(), 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 ); + gameRenderWorld->DrawText(va("\n%1.2f", 1.0f / body->GetInverseMass()), body->GetWorldOrigin(), 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1); } } - if ( af_showTotalMass.GetBool() ) { + if (af_showTotalMass.GetBool()) { axis = gameLocal.GetLocalPlayer()->viewAngles.ToMat3(); - gameRenderWorld->DrawText( va( "\n%1.2f", totalMass ), bodies[0]->GetWorldOrigin() + axis[2] * 8.0f, 0.15f, colorCyan, axis, 1 ); + gameRenderWorld->DrawText(va("\n%1.2f", totalMass), bodies[0]->GetWorldOrigin() + axis[2] * 8.0f, 0.15f, colorCyan, axis, 1); } - if ( af_showInertia.GetBool() ) { - for ( i = 0; i < bodies.Num(); i++ ) { + if (af_showInertia.GetBool()) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; - idMat3 &I = body->inertiaTensor; - gameRenderWorld->DrawText( va( "\n\n\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )", - I[0].x, I[0].y, I[0].z, - I[1].x, I[1].y, I[1].z, - I[2].x, I[2].y, I[2].z ), - body->GetWorldOrigin(), 0.05f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 ); + idMat3& I = body->inertiaTensor; + gameRenderWorld->DrawText(va("\n\n\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )", + I[0].x, I[0].y, I[0].z, + I[1].x, I[1].y, I[1].z, + I[2].x, I[2].y, I[2].z), + body->GetWorldOrigin(), 0.05f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1); } } - if ( af_showVelocity.GetBool() ) { - for ( i = 0; i < bodies.Num(); i++ ) { - DrawVelocity( bodies[i]->clipModel->GetId(), 0.1f, 4.0f ); + if (af_showVelocity.GetBool()) { + for (i = 0; i < bodies.Num(); i++) { + DrawVelocity(bodies[i]->clipModel->GetId(), 0.1f, 4.0f); } } - if ( af_showConstraints.GetBool() ) { - for ( i = 0; i < primaryConstraints.Num(); i++ ) { + if (af_showConstraints.GetBool()) { + for (i = 0; i < primaryConstraints.Num(); i++) { constraint = primaryConstraints[i]; constraint->DebugDraw(); } - if ( !af_showPrimaryOnly.GetBool() ) { - for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) { + if (!af_showPrimaryOnly.GetBool()) { + for (i = 0; i < auxiliaryConstraints.Num(); i++) { constraint = auxiliaryConstraints[i]; constraint->DebugDraw(); } } } - if ( af_showConstraintNames.GetBool() ) { - for ( i = 0; i < primaryConstraints.Num(); i++ ) { + if (af_showConstraintNames.GetBool()) { + for (i = 0; i < primaryConstraints.Num(); i++) { constraint = primaryConstraints[i]; - constraint->GetCenter( center ); - gameRenderWorld->DrawText( constraint->GetName().c_str(), center, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 ); + constraint->GetCenter(center); + gameRenderWorld->DrawText(constraint->GetName().c_str(), center, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1); } - if ( !af_showPrimaryOnly.GetBool() ) { - for ( i = 0; i < auxiliaryConstraints.Num(); i++ ) { + if (!af_showPrimaryOnly.GetBool()) { + for (i = 0; i < auxiliaryConstraints.Num(); i++) { constraint = auxiliaryConstraints[i]; - constraint->GetCenter( center ); - gameRenderWorld->DrawText( constraint->GetName().c_str(), center, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 ); + constraint->GetCenter(center); + gameRenderWorld->DrawText(constraint->GetName().c_str(), center, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1); } } } - if ( af_showTrees.GetBool() || ( af_showActive.GetBool() && current.atRest < 0 ) ) { - for ( i = 0; i < trees.Num(); i++ ) { - trees[i]->DebugDraw( idStr::ColorForIndex( i+3 ) ); + if (af_showTrees.GetBool() || (af_showActive.GetBool() && current.atRest < 0)) { + for (i = 0; i < trees.Num(); i++) { + trees[i]->DebugDraw(idStr::ColorForIndex(i + 3)); } } } @@ -6584,7 +6625,7 @@ void idPhysics_AF::DebugDraw( void ) { idPhysics_AF::idPhysics_AF ================ */ -idPhysics_AF::idPhysics_AF( void ) { +idPhysics_AF::idPhysics_AF(void) { trees.Clear(); bodies.Clear(); constraints.Clear(); @@ -6598,9 +6639,9 @@ idPhysics_AF::idPhysics_AF( void ) { lcp = idLCP::AllocSymmetric(); - memset( ¤t, 0, sizeof( current ) ); + memset(¤t, 0, sizeof(current)); current.atRest = -1; - current.lastTimeStep = USERCMD_MSEC; + current.lastTimeStep = gameLocal.msec; saved = current; linearFriction = 0.005f; @@ -6610,8 +6651,8 @@ idPhysics_AF::idPhysics_AF( void ) { totalMass = 0.0f; forceTotalMass = -1.0f; - suspendVelocity.Set( SUSPEND_LINEAR_VELOCITY, SUSPEND_ANGULAR_VELOCITY ); - suspendAcceleration.Set( SUSPEND_LINEAR_ACCELERATION, SUSPEND_LINEAR_ACCELERATION ); + suspendVelocity.Set(SUSPEND_LINEAR_VELOCITY, SUSPEND_ANGULAR_VELOCITY); + suspendAcceleration.Set(SUSPEND_LINEAR_ACCELERATION, SUSPEND_LINEAR_ACCELERATION); noMoveTime = NO_MOVE_TIME; noMoveTranslation = NO_MOVE_TRANSLATION_TOLERANCE; noMoveRotation = NO_MOVE_ROTATION_TOLERANCE; @@ -6653,27 +6694,24 @@ idPhysics_AF::idPhysics_AF( void ) { idPhysics_AF::~idPhysics_AF ================ */ -idPhysics_AF::~idPhysics_AF( void ) { +idPhysics_AF::~idPhysics_AF(void) { int i; - trees.DeleteContents( true ); + trees.DeleteContents(true); - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { delete bodies[i]; } - for ( i = 0; i < constraints.Num(); i++ ) { + for (i = 0; i < constraints.Num(); i++) { delete constraints[i]; } - contactConstraints.SetNum( contactConstraints.NumAllocated(), false ); - for ( i = 0; i < contactConstraints.NumAllocated(); i++ ) { - delete contactConstraints[i]; - } + contactConstraints.DeleteContents(true); delete lcp; - if ( masterBody ) { + if (masterBody) { delete masterBody; } } @@ -6683,12 +6721,12 @@ idPhysics_AF::~idPhysics_AF( void ) { idPhysics_AF_SavePState ================ */ -void idPhysics_AF_SavePState( idSaveGame *saveFile, const AFPState_t &state ) { - saveFile->WriteInt( state.atRest ); - saveFile->WriteFloat( state.noMoveTime ); - saveFile->WriteFloat( state.activateTime ); - saveFile->WriteFloat( state.lastTimeStep ); - saveFile->WriteVec6( state.pushVelocity ); +void idPhysics_AF_SavePState(idSaveGame* saveFile, const AFPState_t& state) { + saveFile->WriteInt(state.atRest); + saveFile->WriteFloat(state.noMoveTime); + saveFile->WriteFloat(state.activateTime); + saveFile->WriteFloat(state.lastTimeStep); + saveFile->WriteVec6(state.pushVelocity); } /* @@ -6696,12 +6734,12 @@ void idPhysics_AF_SavePState( idSaveGame *saveFile, const AFPState_t &state ) { idPhysics_AF_RestorePState ================ */ -void idPhysics_AF_RestorePState( idRestoreGame *saveFile, AFPState_t &state ) { - saveFile->ReadInt( state.atRest ); - saveFile->ReadFloat( state.noMoveTime ); - saveFile->ReadFloat( state.activateTime ); - saveFile->ReadFloat( state.lastTimeStep ); - saveFile->ReadVec6( state.pushVelocity ); +void idPhysics_AF_RestorePState(idRestoreGame* saveFile, AFPState_t& state) { + saveFile->ReadInt(state.atRest); + saveFile->ReadFloat(state.noMoveTime); + saveFile->ReadFloat(state.activateTime); + saveFile->ReadFloat(state.lastTimeStep); + saveFile->ReadVec6(state.pushVelocity); } /* @@ -6709,71 +6747,72 @@ void idPhysics_AF_RestorePState( idRestoreGame *saveFile, AFPState_t &state ) { idPhysics_AF::Save ================ */ -void idPhysics_AF::Save( idSaveGame *saveFile ) const { +void idPhysics_AF::Save(idSaveGame* saveFile) const { int i; // the articulated figure structure is handled by the owner - idPhysics_AF_SavePState( saveFile, current ); - idPhysics_AF_SavePState( saveFile, saved ); + idPhysics_AF_SavePState(saveFile, current); + idPhysics_AF_SavePState(saveFile, saved); - saveFile->WriteInt( bodies.Num() ); - for ( i = 0; i < bodies.Num(); i++ ) { - bodies[i]->Save( saveFile ); + saveFile->WriteInt(bodies.Num()); + for (i = 0; i < bodies.Num(); i++) { + bodies[i]->Save(saveFile); } - if ( masterBody ) { - saveFile->WriteBool( true ); - masterBody->Save( saveFile ); - } else { - saveFile->WriteBool( false ); + if (masterBody) { + saveFile->WriteBool(true); + masterBody->Save(saveFile); + } + else { + saveFile->WriteBool(false); } - saveFile->WriteInt( constraints.Num() ); - for ( i = 0; i < constraints.Num(); i++ ) { - constraints[i]->Save( saveFile ); + saveFile->WriteInt(constraints.Num()); + for (i = 0; i < constraints.Num(); i++) { + constraints[i]->Save(saveFile); } - saveFile->WriteBool( changedAF ); + saveFile->WriteBool(changedAF); - saveFile->WriteFloat( linearFriction ); - saveFile->WriteFloat( angularFriction ); - saveFile->WriteFloat( contactFriction ); - saveFile->WriteFloat( bouncyness ); - saveFile->WriteFloat( totalMass ); - saveFile->WriteFloat( forceTotalMass ); + saveFile->WriteFloat(linearFriction); + saveFile->WriteFloat(angularFriction); + saveFile->WriteFloat(contactFriction); + saveFile->WriteFloat(bouncyness); + saveFile->WriteFloat(totalMass); + saveFile->WriteFloat(forceTotalMass); - saveFile->WriteVec2( suspendVelocity ); - saveFile->WriteVec2( suspendAcceleration ); - saveFile->WriteFloat( noMoveTime ); - saveFile->WriteFloat( noMoveTranslation ); - saveFile->WriteFloat( noMoveRotation ); - saveFile->WriteFloat( minMoveTime ); - saveFile->WriteFloat( maxMoveTime ); - saveFile->WriteFloat( impulseThreshold ); + saveFile->WriteVec2(suspendVelocity); + saveFile->WriteVec2(suspendAcceleration); + saveFile->WriteFloat(noMoveTime); + saveFile->WriteFloat(noMoveTranslation); + saveFile->WriteFloat(noMoveRotation); + saveFile->WriteFloat(minMoveTime); + saveFile->WriteFloat(maxMoveTime); + saveFile->WriteFloat(impulseThreshold); - saveFile->WriteFloat( timeScale ); - saveFile->WriteFloat( timeScaleRampStart ); - saveFile->WriteFloat( timeScaleRampEnd ); + saveFile->WriteFloat(timeScale); + saveFile->WriteFloat(timeScaleRampStart); + saveFile->WriteFloat(timeScaleRampEnd); - saveFile->WriteFloat( jointFrictionScale ); - saveFile->WriteFloat( jointFrictionDent ); - saveFile->WriteFloat( jointFrictionDentStart ); - saveFile->WriteFloat( jointFrictionDentEnd ); - saveFile->WriteFloat( jointFrictionDentScale ); + saveFile->WriteFloat(jointFrictionScale); + saveFile->WriteFloat(jointFrictionDent); + saveFile->WriteFloat(jointFrictionDentStart); + saveFile->WriteFloat(jointFrictionDentEnd); + saveFile->WriteFloat(jointFrictionDentScale); - saveFile->WriteFloat( contactFrictionScale ); - saveFile->WriteFloat( contactFrictionDent ); - saveFile->WriteFloat( contactFrictionDentStart ); - saveFile->WriteFloat( contactFrictionDentEnd ); - saveFile->WriteFloat( contactFrictionDentScale ); + saveFile->WriteFloat(contactFrictionScale); + saveFile->WriteFloat(contactFrictionDent); + saveFile->WriteFloat(contactFrictionDentStart); + saveFile->WriteFloat(contactFrictionDentEnd); + saveFile->WriteFloat(contactFrictionDentScale); - saveFile->WriteBool( enableCollision ); - saveFile->WriteBool( selfCollision ); - saveFile->WriteBool( comeToRest ); - saveFile->WriteBool( linearTime ); - saveFile->WriteBool( noImpact ); - saveFile->WriteBool( worldConstraintsLocked ); - saveFile->WriteBool( forcePushable ); + saveFile->WriteBool(enableCollision); + saveFile->WriteBool(selfCollision); + saveFile->WriteBool(comeToRest); + saveFile->WriteBool(linearTime); + saveFile->WriteBool(noImpact); + saveFile->WriteBool(worldConstraintsLocked); + saveFile->WriteBool(forcePushable); } /* @@ -6781,73 +6820,73 @@ void idPhysics_AF::Save( idSaveGame *saveFile ) const { idPhysics_AF::Restore ================ */ -void idPhysics_AF::Restore( idRestoreGame *saveFile ) { +void idPhysics_AF::Restore(idRestoreGame* saveFile) { int i, num; bool hasMaster; // the articulated figure structure should have already been restored - idPhysics_AF_RestorePState( saveFile, current ); - idPhysics_AF_RestorePState( saveFile, saved ); + idPhysics_AF_RestorePState(saveFile, current); + idPhysics_AF_RestorePState(saveFile, saved); - saveFile->ReadInt( num ); - assert( num == bodies.Num() ); - for ( i = 0; i < bodies.Num(); i++ ) { - bodies[i]->Restore( saveFile ); + saveFile->ReadInt(num); + assert(num == bodies.Num()); + for (i = 0; i < bodies.Num(); i++) { + bodies[i]->Restore(saveFile); } - saveFile->ReadBool( hasMaster ); - if ( hasMaster ) { + saveFile->ReadBool(hasMaster); + if (hasMaster) { masterBody = new idAFBody(); - masterBody->Restore( saveFile ); + masterBody->Restore(saveFile); } - saveFile->ReadInt( num ); - assert( num == constraints.Num() ); - for ( i = 0; i < constraints.Num(); i++ ) { - constraints[i]->Restore( saveFile ); + saveFile->ReadInt(num); + assert(num == constraints.Num()); + for (i = 0; i < constraints.Num(); i++) { + constraints[i]->Restore(saveFile); } - saveFile->ReadBool( changedAF ); + saveFile->ReadBool(changedAF); - saveFile->ReadFloat( linearFriction ); - saveFile->ReadFloat( angularFriction ); - saveFile->ReadFloat( contactFriction ); - saveFile->ReadFloat( bouncyness ); - saveFile->ReadFloat( totalMass ); - saveFile->ReadFloat( forceTotalMass ); + saveFile->ReadFloat(linearFriction); + saveFile->ReadFloat(angularFriction); + saveFile->ReadFloat(contactFriction); + saveFile->ReadFloat(bouncyness); + saveFile->ReadFloat(totalMass); + saveFile->ReadFloat(forceTotalMass); - saveFile->ReadVec2( suspendVelocity ); - saveFile->ReadVec2( suspendAcceleration ); - saveFile->ReadFloat( noMoveTime ); - saveFile->ReadFloat( noMoveTranslation ); - saveFile->ReadFloat( noMoveRotation ); - saveFile->ReadFloat( minMoveTime ); - saveFile->ReadFloat( maxMoveTime ); - saveFile->ReadFloat( impulseThreshold ); + saveFile->ReadVec2(suspendVelocity); + saveFile->ReadVec2(suspendAcceleration); + saveFile->ReadFloat(noMoveTime); + saveFile->ReadFloat(noMoveTranslation); + saveFile->ReadFloat(noMoveRotation); + saveFile->ReadFloat(minMoveTime); + saveFile->ReadFloat(maxMoveTime); + saveFile->ReadFloat(impulseThreshold); - saveFile->ReadFloat( timeScale ); - saveFile->ReadFloat( timeScaleRampStart ); - saveFile->ReadFloat( timeScaleRampEnd ); + saveFile->ReadFloat(timeScale); + saveFile->ReadFloat(timeScaleRampStart); + saveFile->ReadFloat(timeScaleRampEnd); - saveFile->ReadFloat( jointFrictionScale ); - saveFile->ReadFloat( jointFrictionDent ); - saveFile->ReadFloat( jointFrictionDentStart ); - saveFile->ReadFloat( jointFrictionDentEnd ); - saveFile->ReadFloat( jointFrictionDentScale ); + saveFile->ReadFloat(jointFrictionScale); + saveFile->ReadFloat(jointFrictionDent); + saveFile->ReadFloat(jointFrictionDentStart); + saveFile->ReadFloat(jointFrictionDentEnd); + saveFile->ReadFloat(jointFrictionDentScale); - saveFile->ReadFloat( contactFrictionScale ); - saveFile->ReadFloat( contactFrictionDent ); - saveFile->ReadFloat( contactFrictionDentStart ); - saveFile->ReadFloat( contactFrictionDentEnd ); - saveFile->ReadFloat( contactFrictionDentScale ); + saveFile->ReadFloat(contactFrictionScale); + saveFile->ReadFloat(contactFrictionDent); + saveFile->ReadFloat(contactFrictionDentStart); + saveFile->ReadFloat(contactFrictionDentEnd); + saveFile->ReadFloat(contactFrictionDentScale); - saveFile->ReadBool( enableCollision ); - saveFile->ReadBool( selfCollision ); - saveFile->ReadBool( comeToRest ); - saveFile->ReadBool( linearTime ); - saveFile->ReadBool( noImpact ); - saveFile->ReadBool( worldConstraintsLocked ); - saveFile->ReadBool( forcePushable ); + saveFile->ReadBool(enableCollision); + saveFile->ReadBool(selfCollision); + saveFile->ReadBool(comeToRest); + saveFile->ReadBool(linearTime); + saveFile->ReadBool(noImpact); + saveFile->ReadBool(worldConstraintsLocked); + saveFile->ReadBool(forcePushable); changedAF = true; @@ -6859,14 +6898,14 @@ void idPhysics_AF::Restore( idRestoreGame *saveFile ) { idPhysics_AF::IsClosedLoop ================ */ -bool idPhysics_AF::IsClosedLoop( const idAFBody *body1, const idAFBody *body2 ) const { - const idAFBody *b1, *b2; +bool idPhysics_AF::IsClosedLoop(const idAFBody* body1, const idAFBody* body2) const { + const idAFBody* b1, * b2; - for ( b1 = body1; b1->parent; b1 = b1->parent ) { + for (b1 = body1; b1->parent; b1 = b1->parent) { } - for ( b2 = body2; b2->parent; b2 = b2->parent ) { + for (b2 = body2; b2->parent; b2 = b2->parent) { } - return ( b1 == b2 ); + return (b1 == b2); } /* @@ -6874,31 +6913,31 @@ bool idPhysics_AF::IsClosedLoop( const idAFBody *body1, const idAFBody *body2 ) idPhysics_AF::BuildTrees ================ */ -void idPhysics_AF::BuildTrees( void ) { +void idPhysics_AF::BuildTrees(void) { int i; float scale; - idAFBody *b; - idAFConstraint *c; - idAFTree *tree; + idAFBody* b; + idAFConstraint* c; + idAFTree* tree; primaryConstraints.Clear(); auxiliaryConstraints.Clear(); - trees.DeleteContents( true ); + trees.DeleteContents(true); totalMass = 0.0f; - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { b = bodies[i]; b->parent = NULL; b->primaryConstraint = NULL; - b->constraints.SetNum( 0, false ); + b->constraints.SetNum(0, false); b->children.Clear(); b->tree = NULL; totalMass += b->mass; } - if ( forceTotalMass > 0.0f ) { + if (forceTotalMass > 0.0f) { scale = forceTotalMass / totalMass; - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { b = bodies[i]; b->mass *= scale; b->invMass = 1.0f / b->mass; @@ -6908,83 +6947,85 @@ void idPhysics_AF::BuildTrees( void ) { totalMass = forceTotalMass; } - if ( af_useLinearTime.GetBool() ) { + if (af_useLinearTime.GetBool()) { - for ( i = 0; i < constraints.Num(); i++ ) { + for (i = 0; i < constraints.Num(); i++) { c = constraints[i]; - c->body1->constraints.Append( c ); - if ( c->body2 ) { - c->body2->constraints.Append( c ); + c->body1->constraints.Append(c); + if (c->body2) { + c->body2->constraints.Append(c); } // only bilateral constraints between two non-world bodies that do not // create loops can be used as primary constraints - if ( !c->body1->primaryConstraint && c->fl.allowPrimary && c->body2 != NULL && !IsClosedLoop( c->body1, c->body2 ) ) { + if (!c->body1->primaryConstraint && c->fl.allowPrimary && c->body2 != NULL && !IsClosedLoop(c->body1, c->body2)) { c->body1->primaryConstraint = c; c->body1->parent = c->body2; - c->body2->children.Append( c->body1 ); + c->body2->children.Append(c->body1); c->fl.isPrimary = true; c->firstIndex = 0; - primaryConstraints.Append( c ); - } else { + primaryConstraints.Append(c); + } + else { c->fl.isPrimary = false; - auxiliaryConstraints.Append( c ); + auxiliaryConstraints.Append(c); } } // create trees for all parent bodies - for ( i = 0; i < bodies.Num(); i++ ) { - if ( !bodies[i]->parent ) { + for (i = 0; i < bodies.Num(); i++) { + if (!bodies[i]->parent) { tree = new idAFTree(); tree->sortedBodies.Clear(); - tree->sortedBodies.Append( bodies[i] ); + tree->sortedBodies.Append(bodies[i]); bodies[i]->tree = tree; - trees.Append( tree ); + trees.Append(tree); } } // add each child body to the appropriate tree - for ( i = 0; i < bodies.Num(); i++ ) { - if ( bodies[i]->parent ) { - for ( b = bodies[i]->parent; !b->tree; b = b->parent ) { + for (i = 0; i < bodies.Num(); i++) { + if (bodies[i]->parent) { + for (b = bodies[i]->parent; !b->tree; b = b->parent) { } - b->tree->sortedBodies.Append( bodies[i] ); + b->tree->sortedBodies.Append(bodies[i]); bodies[i]->tree = b->tree; } } - if ( trees.Num() > 1 ) { - gameLocal.Warning( "Articulated figure has multiple seperate tree structures for entity '%s' type '%s'.", - self->name.c_str(), self->GetType()->classname ); + if (trees.Num() > 1) { + gameLocal.Warning("Articulated figure has multiple seperate tree structures for entity '%s' type '%s'.", + self->name.c_str(), self->GetType()->classname); } // sort bodies in each tree to make sure parents come first - for ( i = 0; i < trees.Num(); i++ ) { + for (i = 0; i < trees.Num(); i++) { trees[i]->SortBodies(); } - } else { + } + else { // create a tree for each body - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { tree = new idAFTree(); tree->sortedBodies.Clear(); - tree->sortedBodies.Append( bodies[i] ); + tree->sortedBodies.Append(bodies[i]); bodies[i]->tree = tree; - trees.Append( tree ); + trees.Append(tree); } - for ( i = 0; i < constraints.Num(); i++ ) { + for (i = 0; i < constraints.Num(); i++) { c = constraints[i]; - c->body1->constraints.Append( c ); - if ( c->body2 ) { - c->body2->constraints.Append( c ); + c->body1->constraints.Append(c); + if (c->body2) { + c->body2->constraints.Append(c); } c->fl.isPrimary = false; - auxiliaryConstraints.Append( c ); + auxiliaryConstraints.Append(c); } } } @@ -6997,36 +7038,36 @@ idPhysics_AF::AddBody as such the first body added will get id zero ================ */ -int idPhysics_AF::AddBody( idAFBody *body ) { +int idPhysics_AF::AddBody(idAFBody* body) { int id = 0; - if ( !body->clipModel ) { - gameLocal.Error( "idPhysics_AF::AddBody: body '%s' has no clip model.", body->name.c_str() ); + if (!body->clipModel) { + gameLocal.Error("idPhysics_AF::AddBody: body '%s' has no clip model.", body->name.c_str()); } - if ( bodies.Find( body ) ) { - gameLocal.Error( "idPhysics_AF::AddBody: body '%s' added twice.", body->name.c_str() ); + if (bodies.Find(body)) { + gameLocal.Error("idPhysics_AF::AddBody: body '%s' added twice.", body->name.c_str()); } - if ( GetBody( body->name ) ) { - gameLocal.Error( "idPhysics_AF::AddBody: a body with the name '%s' already exists.", body->name.c_str() ); + if (GetBody(body->name)) { + gameLocal.Error("idPhysics_AF::AddBody: a body with the name '%s' already exists.", body->name.c_str()); } id = bodies.Num(); - body->clipModel->SetId( id ); - if ( body->linearFriction < 0.0f ) { + body->clipModel->SetId(id); + if (body->linearFriction < 0.0f) { body->linearFriction = linearFriction; body->angularFriction = angularFriction; body->contactFriction = contactFriction; } - if ( body->bouncyness < 0.0f ) { + if (body->bouncyness < 0.0f) { body->bouncyness = bouncyness; } - if ( !body->fl.clipMaskSet ) { + if (!body->fl.clipMaskSet) { body->clipMask = clipMask; } - bodies.Append( body ); + bodies.Append(body); changedAF = true; @@ -7038,28 +7079,28 @@ int idPhysics_AF::AddBody( idAFBody *body ) { idPhysics_AF::AddConstraint ================ */ -void idPhysics_AF::AddConstraint( idAFConstraint *constraint ) { +void idPhysics_AF::AddConstraint(idAFConstraint* constraint) { - if ( constraints.Find( constraint ) ) { - gameLocal.Error( "idPhysics_AF::AddConstraint: constraint '%s' added twice.", constraint->name.c_str() ); + if (constraints.Find(constraint)) { + gameLocal.Error("idPhysics_AF::AddConstraint: constraint '%s' added twice.", constraint->name.c_str()); } - if ( GetConstraint( constraint->name ) ) { - gameLocal.Error( "idPhysics_AF::AddConstraint: a constraint with the name '%s' already exists.", constraint->name.c_str() ); + if (GetConstraint(constraint->name)) { + gameLocal.Error("idPhysics_AF::AddConstraint: a constraint with the name '%s' already exists.", constraint->name.c_str()); } - if ( !constraint->body1 ) { - gameLocal.Error( "idPhysics_AF::AddConstraint: body1 == NULL on constraint '%s'.", constraint->name.c_str() ); + if (!constraint->body1) { + gameLocal.Error("idPhysics_AF::AddConstraint: body1 == NULL on constraint '%s'.", constraint->name.c_str()); } - if ( !bodies.Find( constraint->body1 ) ) { - gameLocal.Error( "idPhysics_AF::AddConstraint: body1 of constraint '%s' is not part of the articulated figure.", constraint->name.c_str() ); + if (!bodies.Find(constraint->body1)) { + gameLocal.Error("idPhysics_AF::AddConstraint: body1 of constraint '%s' is not part of the articulated figure.", constraint->name.c_str()); } - if ( constraint->body2 && !bodies.Find( constraint->body2 ) ) { - gameLocal.Error( "idPhysics_AF::AddConstraint: body2 of constraint '%s' is not part of the articulated figure.", constraint->name.c_str() ); + if (constraint->body2 && !bodies.Find(constraint->body2)) { + gameLocal.Error("idPhysics_AF::AddConstraint: body2 of constraint '%s' is not part of the articulated figure.", constraint->name.c_str()); } - if ( constraint->body1 == constraint->body2 ) { - gameLocal.Error( "idPhysics_AF::AddConstraint: body1 and body2 of constraint '%s' are the same.", constraint->name.c_str() ); + if (constraint->body1 == constraint->body2) { + gameLocal.Error("idPhysics_AF::AddConstraint: body1 and body2 of constraint '%s' are the same.", constraint->name.c_str()); } - constraints.Append( constraint ); + constraints.Append(constraint); constraint->physics = this; changedAF = true; @@ -7070,8 +7111,8 @@ void idPhysics_AF::AddConstraint( idAFConstraint *constraint ) { idPhysics_AF::AddFrameConstraint ================ */ -void idPhysics_AF::AddFrameConstraint( idAFConstraint *constraint ) { - frameConstraints.Append( constraint ); +void idPhysics_AF::AddFrameConstraint(idAFConstraint* constraint) { + frameConstraints.Append(constraint); constraint->physics = this; } @@ -7080,15 +7121,15 @@ void idPhysics_AF::AddFrameConstraint( idAFConstraint *constraint ) { idPhysics_AF::ForceBodyId ================ */ -void idPhysics_AF::ForceBodyId( idAFBody *body, int newId ) { +void idPhysics_AF::ForceBodyId(idAFBody* body, int newId) { int id; - id = bodies.FindIndex( body ); - if ( id == -1 ) { - gameLocal.Error( "ForceBodyId: body '%s' is not part of the articulated figure.\n", body->name.c_str() ); + id = bodies.FindIndex(body); + if (id == -1) { + gameLocal.Error("ForceBodyId: body '%s' is not part of the articulated figure.\n", body->name.c_str()); } - if ( id != newId ) { - idAFBody *b = bodies[newId]; + if (id != newId) { + idAFBody* b = bodies[newId]; bodies[newId] = bodies[id]; bodies[id] = b; changedAF = true; @@ -7100,12 +7141,12 @@ void idPhysics_AF::ForceBodyId( idAFBody *body, int newId ) { idPhysics_AF::GetBodyId ================ */ -int idPhysics_AF::GetBodyId( idAFBody *body ) const { +int idPhysics_AF::GetBodyId(idAFBody* body) const { int id; - id = bodies.FindIndex( body ); - if ( id == -1 && body ) { - gameLocal.Error( "GetBodyId: body '%s' is not part of the articulated figure.\n", body->name.c_str() ); + id = bodies.FindIndex(body); + if (id == -1 && body) { + gameLocal.Error("GetBodyId: body '%s' is not part of the articulated figure.\n", body->name.c_str()); } return id; } @@ -7115,15 +7156,15 @@ int idPhysics_AF::GetBodyId( idAFBody *body ) const { idPhysics_AF::GetBodyId ================ */ -int idPhysics_AF::GetBodyId( const char *bodyName ) const { +int idPhysics_AF::GetBodyId(const char* bodyName) const { int i; - for ( i = 0; i < bodies.Num(); i++ ) { - if ( !bodies[i]->name.Icmp( bodyName ) ) { + for (i = 0; i < bodies.Num(); i++) { + if (!bodies[i]->name.Icmp(bodyName)) { return i; } } - gameLocal.Error( "GetBodyId: no body with the name '%s' is not part of the articulated figure.\n", bodyName ); + gameLocal.Error("GetBodyId: no body with the name '%s' is not part of the articulated figure.\n", bodyName); return 0; } @@ -7132,12 +7173,12 @@ int idPhysics_AF::GetBodyId( const char *bodyName ) const { idPhysics_AF::GetConstraintId ================ */ -int idPhysics_AF::GetConstraintId( idAFConstraint *constraint ) const { +int idPhysics_AF::GetConstraintId(idAFConstraint* constraint) const { int id; - id = constraints.FindIndex( constraint ); - if ( id == -1 && constraint ) { - gameLocal.Error( "GetConstraintId: constraint '%s' is not part of the articulated figure.\n", constraint->name.c_str() ); + id = constraints.FindIndex(constraint); + if (id == -1 && constraint) { + gameLocal.Error("GetConstraintId: constraint '%s' is not part of the articulated figure.\n", constraint->name.c_str()); } return id; } @@ -7147,15 +7188,15 @@ int idPhysics_AF::GetConstraintId( idAFConstraint *constraint ) const { idPhysics_AF::GetConstraintId ================ */ -int idPhysics_AF::GetConstraintId( const char *constraintName ) const { +int idPhysics_AF::GetConstraintId(const char* constraintName) const { int i; - for ( i = 0; i < constraints.Num(); i++ ) { - if ( constraints[i]->name.Icmp( constraintName ) == 0 ) { + for (i = 0; i < constraints.Num(); i++) { + if (constraints[i]->name.Icmp(constraintName) == 0) { return i; } } - gameLocal.Error( "GetConstraintId: no constraint with the name '%s' is not part of the articulated figure.\n", constraintName ); + gameLocal.Error("GetConstraintId: no constraint with the name '%s' is not part of the articulated figure.\n", constraintName); return 0; } @@ -7164,7 +7205,7 @@ int idPhysics_AF::GetConstraintId( const char *constraintName ) const { idPhysics_AF::GetNumBodies ================ */ -int idPhysics_AF::GetNumBodies( void ) const { +int idPhysics_AF::GetNumBodies(void) const { return bodies.Num(); } @@ -7173,7 +7214,7 @@ int idPhysics_AF::GetNumBodies( void ) const { idPhysics_AF::GetNumConstraints ================ */ -int idPhysics_AF::GetNumConstraints( void ) const { +int idPhysics_AF::GetNumConstraints(void) const { return constraints.Num(); } @@ -7182,11 +7223,11 @@ int idPhysics_AF::GetNumConstraints( void ) const { idPhysics_AF::GetBody ================ */ -idAFBody *idPhysics_AF::GetBody( const char *bodyName ) const { +idAFBody* idPhysics_AF::GetBody(const char* bodyName) const { int i; - for ( i = 0; i < bodies.Num(); i++ ) { - if ( !bodies[i]->name.Icmp( bodyName ) ) { + for (i = 0; i < bodies.Num(); i++) { + if (!bodies[i]->name.Icmp(bodyName)) { return bodies[i]; } } @@ -7199,9 +7240,9 @@ idAFBody *idPhysics_AF::GetBody( const char *bodyName ) const { idPhysics_AF::GetBody ================ */ -idAFBody *idPhysics_AF::GetBody( const int id ) const { - if ( id < 0 || id >= bodies.Num() ) { - gameLocal.Error( "GetBody: no body with id %d exists\n", id ); +idAFBody* idPhysics_AF::GetBody(const int id) const { + if (id < 0 || id >= bodies.Num()) { + gameLocal.Error("GetBody: no body with id %d exists\n", id); return NULL; } return bodies[id]; @@ -7212,11 +7253,11 @@ idAFBody *idPhysics_AF::GetBody( const int id ) const { idPhysics_AF::GetConstraint ================ */ -idAFConstraint *idPhysics_AF::GetConstraint( const char *constraintName ) const { +idAFConstraint* idPhysics_AF::GetConstraint(const char* constraintName) const { int i; - for ( i = 0; i < constraints.Num(); i++ ) { - if ( constraints[i]->name.Icmp( constraintName ) == 0 ) { + for (i = 0; i < constraints.Num(); i++) { + if (constraints[i]->name.Icmp(constraintName) == 0) { return constraints[i]; } } @@ -7229,9 +7270,9 @@ idAFConstraint *idPhysics_AF::GetConstraint( const char *constraintName ) const idPhysics_AF::GetConstraint ================ */ -idAFConstraint *idPhysics_AF::GetConstraint( const int id ) const { - if ( id < 0 || id >= constraints.Num() ) { - gameLocal.Error( "GetConstraint: no constraint with id %d exists\n", id ); +idAFConstraint* idPhysics_AF::GetConstraint(const int id) const { + if (id < 0 || id >= constraints.Num()) { + gameLocal.Error("GetConstraint: no constraint with id %d exists\n", id); return NULL; } return constraints[id]; @@ -7242,23 +7283,23 @@ idAFConstraint *idPhysics_AF::GetConstraint( const int id ) const { idPhysics_AF::DeleteBody ================ */ -void idPhysics_AF::DeleteBody( const char *bodyName ) { +void idPhysics_AF::DeleteBody(const char* bodyName) { int i; // find the body with the given name - for ( i = 0; i < bodies.Num(); i++ ) { - if ( !bodies[i]->name.Icmp( bodyName ) ) { + for (i = 0; i < bodies.Num(); i++) { + if (!bodies[i]->name.Icmp(bodyName)) { break; } } - if ( i >= bodies.Num() ) { - gameLocal.Warning( "DeleteBody: no body found in the articulated figure with the name '%s' for entity '%s' type '%s'.", - bodyName, self->name.c_str(), self->GetType()->classname ); + if (i >= bodies.Num()) { + gameLocal.Warning("DeleteBody: no body found in the articulated figure with the name '%s' for entity '%s' type '%s'.", + bodyName, self->name.c_str(), self->GetType()->classname); return; } - DeleteBody( i ); + DeleteBody(i); } /* @@ -7266,30 +7307,30 @@ void idPhysics_AF::DeleteBody( const char *bodyName ) { idPhysics_AF::DeleteBody ================ */ -void idPhysics_AF::DeleteBody( const int id ) { +void idPhysics_AF::DeleteBody(const int id) { int j; - if ( id < 0 || id > bodies.Num() ) { - gameLocal.Error( "DeleteBody: no body with id %d.", id ); + if (id < 0 || id > bodies.Num()) { + gameLocal.Error("DeleteBody: no body with id %d.", id); return; } // remove any constraints attached to this body - for ( j = 0; j < constraints.Num(); j++ ) { - if ( constraints[j]->body1 == bodies[id] || constraints[j]->body2 == bodies[id] ) { + for (j = 0; j < constraints.Num(); j++) { + if (constraints[j]->body1 == bodies[id] || constraints[j]->body2 == bodies[id]) { delete constraints[j]; - constraints.RemoveIndex( j ); + constraints.RemoveIndex(j); j--; } } // remove the body delete bodies[id]; - bodies.RemoveIndex( id ); + bodies.RemoveIndex(id); // set new body ids - for ( j = 0; j < bodies.Num(); j++ ) { - bodies[j]->clipModel->SetId( j ); + for (j = 0; j < bodies.Num(); j++) { + bodies[j]->clipModel->SetId(j); } changedAF = true; @@ -7300,23 +7341,23 @@ void idPhysics_AF::DeleteBody( const int id ) { idPhysics_AF::DeleteConstraint ================ */ -void idPhysics_AF::DeleteConstraint( const char *constraintName ) { +void idPhysics_AF::DeleteConstraint(const char* constraintName) { int i; // find the constraint with the given name - for ( i = 0; i < constraints.Num(); i++ ) { - if ( !constraints[i]->name.Icmp( constraintName ) ) { + for (i = 0; i < constraints.Num(); i++) { + if (!constraints[i]->name.Icmp(constraintName)) { break; } } - if ( i >= constraints.Num() ) { - gameLocal.Warning( "DeleteConstraint: no constriant found in the articulated figure with the name '%s' for entity '%s' type '%s'.", - constraintName, self->name.c_str(), self->GetType()->classname ); + if (i >= constraints.Num()) { + gameLocal.Warning("DeleteConstraint: no constriant found in the articulated figure with the name '%s' for entity '%s' type '%s'.", + constraintName, self->name.c_str(), self->GetType()->classname); return; } - DeleteConstraint( i ); + DeleteConstraint(i); } /* @@ -7324,16 +7365,16 @@ void idPhysics_AF::DeleteConstraint( const char *constraintName ) { idPhysics_AF::DeleteConstraint ================ */ -void idPhysics_AF::DeleteConstraint( const int id ) { +void idPhysics_AF::DeleteConstraint(const int id) { - if ( id < 0 || id >= constraints.Num() ) { - gameLocal.Error( "DeleteConstraint: no constraint with id %d.", id ); + if (id < 0 || id >= constraints.Num()) { + gameLocal.Error("DeleteConstraint: no constraint with id %d.", id); return; } // remove the constraint delete constraints[id]; - constraints.RemoveIndex( id ); + constraints.RemoveIndex(id); changedAF = true; } @@ -7343,22 +7384,22 @@ void idPhysics_AF::DeleteConstraint( const int id ) { idPhysics_AF::GetBodyContactConstraints ================ */ -int idPhysics_AF::GetBodyContactConstraints( const int id, idAFConstraint_Contact *contacts[], int maxContacts ) const { +int idPhysics_AF::GetBodyContactConstraints(const int id, idAFConstraint_Contact* contacts[], int maxContacts) const { int i, numContacts; - idAFBody *body; - idAFConstraint_Contact *contact; + idAFBody* body; + idAFConstraint_Contact* contact; - if ( id < 0 || id >= bodies.Num() || maxContacts <= 0 ) { + if (id < 0 || id >= bodies.Num() || maxContacts <= 0) { return 0; } numContacts = 0; body = bodies[id]; - for ( i = 0; i < contactConstraints.Num(); i++ ) { + for (i = 0; i < contactConstraints.Num(); i++) { contact = contactConstraints[i]; - if ( contact->body1 == body || contact->body2 == body ) { + if (contact->body1 == body || contact->body2 == body) { contacts[numContacts++] = contact; - if ( numContacts >= maxContacts ) { + if (numContacts >= maxContacts) { return numContacts; } } @@ -7371,10 +7412,10 @@ int idPhysics_AF::GetBodyContactConstraints( const int id, idAFConstraint_Contac idPhysics_AF::SetDefaultFriction ================ */ -void idPhysics_AF::SetDefaultFriction( float linear, float angular, float contact ) { - if ( linear < 0.0f || linear > 1.0f || - angular < 0.0f || angular > 1.0f || - contact < 0.0f || contact > 1.0f ) { +void idPhysics_AF::SetDefaultFriction(float linear, float angular, float contact) { + if (linear < 0.0f || linear > 1.0f || + angular < 0.0f || angular > 1.0f || + contact < 0.0f || contact > 1.0f) { return; } linearFriction = linear; @@ -7387,15 +7428,15 @@ void idPhysics_AF::SetDefaultFriction( float linear, float angular, float contac idPhysics_AF::GetImpactInfo ================ */ -void idPhysics_AF::GetImpactInfo( const int id, const idVec3 &point, impactInfo_t *info ) const { - if ( id < 0 || id >= bodies.Num() ) { - memset( info, 0, sizeof( *info ) ); +void idPhysics_AF::GetImpactInfo(const int id, const idVec3& point, impactInfo_t* info) const { + if (id < 0 || id >= bodies.Num()) { + memset(info, 0, sizeof(*info)); return; } info->invMass = 1.0f / bodies[id]->mass; info->invInertiaTensor = bodies[id]->current->worldAxis.Transpose() * bodies[id]->inverseInertiaTensor * bodies[id]->current->worldAxis; info->position = point - bodies[id]->current->worldOrigin; - info->velocity = bodies[id]->current->spatialVelocity.SubVec3(0) + bodies[id]->current->spatialVelocity.SubVec3(1).Cross( info->position ); + info->velocity = bodies[id]->current->spatialVelocity.SubVec3(0) + bodies[id]->current->spatialVelocity.SubVec3(1).Cross(info->position); } /* @@ -7403,16 +7444,16 @@ void idPhysics_AF::GetImpactInfo( const int id, const idVec3 &point, impactInfo_ idPhysics_AF::ApplyImpulse ================ */ -void idPhysics_AF::ApplyImpulse( const int id, const idVec3 &point, const idVec3 &impulse ) { - if ( id < 0 || id >= bodies.Num() ) { +void idPhysics_AF::ApplyImpulse(const int id, const idVec3& point, const idVec3& impulse) { + if (id < 0 || id >= bodies.Num()) { return; } - if ( noImpact || impulse.LengthSqr() < Square( impulseThreshold ) ) { + if (noImpact || impulse.LengthSqr() < Square(impulseThreshold)) { return; } idMat3 invWorldInertiaTensor = bodies[id]->current->worldAxis.Transpose() * bodies[id]->inverseInertiaTensor * bodies[id]->current->worldAxis; bodies[id]->current->spatialVelocity.SubVec3(0) += bodies[id]->invMass * impulse; - bodies[id]->current->spatialVelocity.SubVec3(1) += invWorldInertiaTensor * (point - bodies[id]->current->worldOrigin).Cross( impulse ); + bodies[id]->current->spatialVelocity.SubVec3(1) += invWorldInertiaTensor * (point - bodies[id]->current->worldOrigin).Cross(impulse); Activate(); } @@ -7421,15 +7462,15 @@ void idPhysics_AF::ApplyImpulse( const int id, const idVec3 &point, const idVec3 idPhysics_AF::AddForce ================ */ -void idPhysics_AF::AddForce( const int id, const idVec3 &point, const idVec3 &force ) { - if ( noImpact ) { +void idPhysics_AF::AddForce(const int id, const idVec3& point, const idVec3& force) { + if (noImpact) { return; } - if ( id < 0 || id >= bodies.Num() ) { + if (id < 0 || id >= bodies.Num()) { return; } - bodies[id]->current->externalForce.SubVec3( 0 ) += force; - bodies[id]->current->externalForce.SubVec3( 1 ) += (point - bodies[id]->current->worldOrigin).Cross( force ); + bodies[id]->current->externalForce.SubVec3(0) += force; + bodies[id]->current->externalForce.SubVec3(1) += (point - bodies[id]->current->worldOrigin).Cross(force); Activate(); } @@ -7438,7 +7479,7 @@ void idPhysics_AF::AddForce( const int id, const idVec3 &point, const idVec3 &fo idPhysics_AF::IsAtRest ================ */ -bool idPhysics_AF::IsAtRest( void ) const { +bool idPhysics_AF::IsAtRest(void) const { return current.atRest >= 0; } @@ -7447,7 +7488,7 @@ bool idPhysics_AF::IsAtRest( void ) const { idPhysics_AF::GetRestStartTime ================ */ -int idPhysics_AF::GetRestStartTime( void ) const { +int idPhysics_AF::GetRestStartTime(void) const { return current.atRest; } @@ -7456,8 +7497,8 @@ int idPhysics_AF::GetRestStartTime( void ) const { idPhysics_AF::IsPushable ================ */ -bool idPhysics_AF::IsPushable( void ) const { - return ( !noImpact && ( masterBody == NULL || forcePushable ) ); +bool idPhysics_AF::IsPushable(void) const { + return (!noImpact && (masterBody == NULL || forcePushable)); } /* @@ -7465,13 +7506,13 @@ bool idPhysics_AF::IsPushable( void ) const { idPhysics_AF::SaveState ================ */ -void idPhysics_AF::SaveState( void ) { +void idPhysics_AF::SaveState(void) { int i; saved = current; - for ( i = 0; i < bodies.Num(); i++ ) { - memcpy( &bodies[i]->saved, bodies[i]->current, sizeof( AFBodyPState_t ) ); + for (i = 0; i < bodies.Num(); i++) { + memcpy(&bodies[i]->saved, bodies[i]->current, sizeof(AFBodyPState_t)); } } @@ -7480,12 +7521,12 @@ void idPhysics_AF::SaveState( void ) { idPhysics_AF::RestoreState ================ */ -void idPhysics_AF::RestoreState( void ) { +void idPhysics_AF::RestoreState(void) { int i; current = saved; - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { *(bodies[i]->current) = bodies[i]->saved; } @@ -7497,11 +7538,16 @@ void idPhysics_AF::RestoreState( void ) { idPhysics_AF::SetOrigin ================ */ -void idPhysics_AF::SetOrigin( const idVec3 &newOrigin, int id ) { - if ( masterBody ) { - Translate( masterBody->current->worldOrigin + masterBody->current->worldAxis * newOrigin - bodies[0]->current->worldOrigin ); - } else { - Translate( newOrigin - bodies[0]->current->worldOrigin ); +void idPhysics_AF::SetOrigin(const idVec3& newOrigin, int id) { + if (!bodies.Num()) { + return; + } + + if (masterBody) { + Translate(masterBody->current->worldOrigin + masterBody->current->worldAxis * newOrigin - bodies[0]->current->worldOrigin); + } + else { + Translate(newOrigin - bodies[0]->current->worldOrigin); } } @@ -7510,19 +7556,24 @@ void idPhysics_AF::SetOrigin( const idVec3 &newOrigin, int id ) { idPhysics_AF::SetAxis ================ */ -void idPhysics_AF::SetAxis( const idMat3 &newAxis, int id ) { +void idPhysics_AF::SetAxis(const idMat3& newAxis, int id) { + if (!bodies.Num()) { + return; + } + idMat3 axis; idRotation rotation; - if ( masterBody ) { - axis = bodies[0]->current->worldAxis.Transpose() * ( newAxis * masterBody->current->worldAxis ); - } else { + if (masterBody) { + axis = bodies[0]->current->worldAxis.Transpose() * (newAxis * masterBody->current->worldAxis); + } + else { axis = bodies[0]->current->worldAxis.Transpose() * newAxis; } rotation = axis.ToRotation(); - rotation.SetOrigin( bodies[0]->current->worldOrigin ); + rotation.SetOrigin(bodies[0]->current->worldOrigin); - Rotate( rotation ); + Rotate(rotation); } /* @@ -7530,19 +7581,19 @@ void idPhysics_AF::SetAxis( const idMat3 &newAxis, int id ) { idPhysics_AF::Translate ================ */ -void idPhysics_AF::Translate( const idVec3 &translation, int id ) { +void idPhysics_AF::Translate(const idVec3& translation, int id) { int i; - idAFBody *body; + idAFBody* body; - if ( !worldConstraintsLocked ) { + if (!worldConstraintsLocked) { // translate constraints attached to the world - for ( i = 0; i < constraints.Num(); i++ ) { - constraints[i]->Translate( translation ); + for (i = 0; i < constraints.Num(); i++) { + constraints[i]->Translate(translation); } } // translate all the bodies - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; body->current->worldOrigin += translation; @@ -7558,19 +7609,19 @@ void idPhysics_AF::Translate( const idVec3 &translation, int id ) { idPhysics_AF::Rotate ================ */ -void idPhysics_AF::Rotate( const idRotation &rotation, int id ) { +void idPhysics_AF::Rotate(const idRotation& rotation, int id) { int i; - idAFBody *body; + idAFBody* body; - if ( !worldConstraintsLocked ) { + if (!worldConstraintsLocked) { // rotate constraints attached to the world - for ( i = 0; i < constraints.Num(); i++ ) { - constraints[i]->Rotate( rotation ); + for (i = 0; i < constraints.Num(); i++) { + constraints[i]->Rotate(rotation); } } // rotate all the bodies - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; body->current->worldOrigin *= rotation; @@ -7587,8 +7638,8 @@ void idPhysics_AF::Rotate( const idRotation &rotation, int id ) { idPhysics_AF::GetOrigin ================ */ -const idVec3 &idPhysics_AF::GetOrigin( int id ) const { - if ( id < 0 || id >= bodies.Num() ) { +const idVec3& idPhysics_AF::GetOrigin(int id) const { + if (id < 0 || id >= bodies.Num()) { return vec3_origin; } else { @@ -7601,8 +7652,8 @@ const idVec3 &idPhysics_AF::GetOrigin( int id ) const { idPhysics_AF::GetAxis ================ */ -const idMat3 &idPhysics_AF::GetAxis( int id ) const { - if ( id < 0 || id >= bodies.Num() ) { +const idMat3& idPhysics_AF::GetAxis(int id) const { + if (id < 0 || id >= bodies.Num()) { return mat3_identity; } else { @@ -7615,11 +7666,11 @@ const idMat3 &idPhysics_AF::GetAxis( int id ) const { idPhysics_AF::SetLinearVelocity ================ */ -void idPhysics_AF::SetLinearVelocity( const idVec3 &newLinearVelocity, int id ) { - if ( id < 0 || id >= bodies.Num() ) { +void idPhysics_AF::SetLinearVelocity(const idVec3& newLinearVelocity, int id) { + if (id < 0 || id >= bodies.Num()) { return; } - bodies[id]->current->spatialVelocity.SubVec3( 0 ) = newLinearVelocity; + bodies[id]->current->spatialVelocity.SubVec3(0) = newLinearVelocity; Activate(); } @@ -7628,11 +7679,11 @@ void idPhysics_AF::SetLinearVelocity( const idVec3 &newLinearVelocity, int id ) idPhysics_AF::SetAngularVelocity ================ */ -void idPhysics_AF::SetAngularVelocity( const idVec3 &newAngularVelocity, int id ) { - if ( id < 0 || id >= bodies.Num() ) { +void idPhysics_AF::SetAngularVelocity(const idVec3& newAngularVelocity, int id) { + if (id < 0 || id >= bodies.Num()) { return; } - bodies[id]->current->spatialVelocity.SubVec3( 1 ) = newAngularVelocity; + bodies[id]->current->spatialVelocity.SubVec3(1) = newAngularVelocity; Activate(); } @@ -7641,12 +7692,12 @@ void idPhysics_AF::SetAngularVelocity( const idVec3 &newAngularVelocity, int id idPhysics_AF::GetLinearVelocity ================ */ -const idVec3 &idPhysics_AF::GetLinearVelocity( int id ) const { - if ( id < 0 || id >= bodies.Num() ) { +const idVec3& idPhysics_AF::GetLinearVelocity(int id) const { + if (id < 0 || id >= bodies.Num()) { return vec3_origin; } else { - return bodies[id]->current->spatialVelocity.SubVec3( 0 ); + return bodies[id]->current->spatialVelocity.SubVec3(0); } } @@ -7655,12 +7706,12 @@ const idVec3 &idPhysics_AF::GetLinearVelocity( int id ) const { idPhysics_AF::GetAngularVelocity ================ */ -const idVec3 &idPhysics_AF::GetAngularVelocity( int id ) const { - if ( id < 0 || id >= bodies.Num() ) { +const idVec3& idPhysics_AF::GetAngularVelocity(int id) const { + if (id < 0 || id >= bodies.Num()) { return vec3_origin; } else { - return bodies[id]->current->spatialVelocity.SubVec3( 1 ); + return bodies[id]->current->spatialVelocity.SubVec3(1); } } @@ -7669,27 +7720,27 @@ const idVec3 &idPhysics_AF::GetAngularVelocity( int id ) const { idPhysics_AF::ClipTranslation ================ */ -void idPhysics_AF::ClipTranslation( trace_t &results, const idVec3 &translation, const idClipModel *model ) const { +void idPhysics_AF::ClipTranslation(trace_t& results, const idVec3& translation, const idClipModel* model) const { int i; - idAFBody *body; + idAFBody* body; trace_t bodyResults; results.fraction = 1.0f; - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; - if ( body->clipModel->IsTraceModel() ) { - if ( model ) { - gameLocal.clip.TranslationModel( bodyResults, body->current->worldOrigin, body->current->worldOrigin + translation, - body->clipModel, body->current->worldAxis, body->clipMask, - model->Handle(), model->GetOrigin(), model->GetAxis() ); + if (body->clipModel->IsTraceModel()) { + if (model) { + gameLocal.clip.TranslationModel(bodyResults, body->current->worldOrigin, body->current->worldOrigin + translation, + body->clipModel, body->current->worldAxis, body->clipMask, + model->Handle(), model->GetOrigin(), model->GetAxis()); } else { - gameLocal.clip.Translation( bodyResults, body->current->worldOrigin, body->current->worldOrigin + translation, - body->clipModel, body->current->worldAxis, body->clipMask, self ); + gameLocal.clip.Translation(bodyResults, body->current->worldOrigin, body->current->worldOrigin + translation, + body->clipModel, body->current->worldAxis, body->clipMask, self); } - if ( bodyResults.fraction < results.fraction ) { + if (bodyResults.fraction < results.fraction) { results = bodyResults; } } @@ -7704,28 +7755,28 @@ void idPhysics_AF::ClipTranslation( trace_t &results, const idVec3 &translation, idPhysics_AF::ClipRotation ================ */ -void idPhysics_AF::ClipRotation( trace_t &results, const idRotation &rotation, const idClipModel *model ) const { +void idPhysics_AF::ClipRotation(trace_t& results, const idRotation& rotation, const idClipModel* model) const { int i; - idAFBody *body; + idAFBody* body; trace_t bodyResults; idRotation partialRotation; results.fraction = 1.0f; - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; - if ( body->clipModel->IsTraceModel() ) { - if ( model ) { - gameLocal.clip.RotationModel( bodyResults, body->current->worldOrigin, rotation, - body->clipModel, body->current->worldAxis, body->clipMask, - model->Handle(), model->GetOrigin(), model->GetAxis() ); + if (body->clipModel->IsTraceModel()) { + if (model) { + gameLocal.clip.RotationModel(bodyResults, body->current->worldOrigin, rotation, + body->clipModel, body->current->worldAxis, body->clipMask, + model->Handle(), model->GetOrigin(), model->GetAxis()); } else { - gameLocal.clip.Rotation( bodyResults, body->current->worldOrigin, rotation, - body->clipModel, body->current->worldAxis, body->clipMask, self ); + gameLocal.clip.Rotation(bodyResults, body->current->worldOrigin, rotation, + body->clipModel, body->current->worldAxis, body->clipMask, self); } - if ( bodyResults.fraction < results.fraction ) { + if (bodyResults.fraction < results.fraction) { results = bodyResults; } } @@ -7741,24 +7792,24 @@ void idPhysics_AF::ClipRotation( trace_t &results, const idRotation &rotation, c idPhysics_AF::ClipContents ================ */ -int idPhysics_AF::ClipContents( const idClipModel *model ) const { +int idPhysics_AF::ClipContents(const idClipModel* model) const { int i, contents; - idAFBody *body; + idAFBody* body; contents = 0; - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { body = bodies[i]; - if ( body->clipModel->IsTraceModel() ) { - if ( model ) { - contents |= gameLocal.clip.ContentsModel( body->current->worldOrigin, - body->clipModel, body->current->worldAxis, -1, - model->Handle(), model->GetOrigin(), model->GetAxis() ); + if (body->clipModel->IsTraceModel()) { + if (model) { + contents |= gameLocal.clip.ContentsModel(body->current->worldOrigin, + body->clipModel, body->current->worldAxis, -1, + model->Handle(), model->GetOrigin(), model->GetAxis()); } else { - contents |= gameLocal.clip.Contents( body->current->worldOrigin, - body->clipModel, body->current->worldAxis, -1, NULL ); + contents |= gameLocal.clip.Contents(body->current->worldOrigin, + body->clipModel, body->current->worldAxis, -1, NULL); } } } @@ -7771,10 +7822,10 @@ int idPhysics_AF::ClipContents( const idClipModel *model ) const { idPhysics_AF::DisableClip ================ */ -void idPhysics_AF::DisableClip( void ) { +void idPhysics_AF::DisableClip(void) { int i; - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { bodies[i]->clipModel->Disable(); } } @@ -7784,10 +7835,10 @@ void idPhysics_AF::DisableClip( void ) { idPhysics_AF::EnableClip ================ */ -void idPhysics_AF::EnableClip( void ) { +void idPhysics_AF::EnableClip(void) { int i; - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { bodies[i]->clipModel->Enable(); } } @@ -7797,10 +7848,10 @@ void idPhysics_AF::EnableClip( void ) { idPhysics_AF::UnlinkClip ================ */ -void idPhysics_AF::UnlinkClip( void ) { +void idPhysics_AF::UnlinkClip(void) { int i; - for ( i = 0; i < bodies.Num(); i++ ) { + for (i = 0; i < bodies.Num(); i++) { bodies[i]->clipModel->Unlink(); } } @@ -7810,7 +7861,7 @@ void idPhysics_AF::UnlinkClip( void ) { idPhysics_AF::LinkClip ================ */ -void idPhysics_AF::LinkClip( void ) { +void idPhysics_AF::LinkClip(void) { UpdateClipModels(); } @@ -7819,17 +7870,17 @@ void idPhysics_AF::LinkClip( void ) { idPhysics_AF::SetPushed ================ */ -void idPhysics_AF::SetPushed( int deltaTime ) { - idAFBody *body; +void idPhysics_AF::SetPushed(int deltaTime) { + idAFBody* body; idRotation rotation; - if ( bodies.Num() ) { + if (bodies.Num()) { body = bodies[0]; - rotation = ( body->saved.worldAxis.Transpose() * body->current->worldAxis ).ToRotation(); + rotation = (body->saved.worldAxis.Transpose() * body->current->worldAxis).ToRotation(); // velocity with which the af is pushed - current.pushVelocity.SubVec3(0) += ( body->current->worldOrigin - body->saved.worldOrigin ) / ( deltaTime * idMath::M_MS2SEC ); - current.pushVelocity.SubVec3(1) += rotation.GetVec() * -DEG2RAD( rotation.GetAngle() ) / ( deltaTime * idMath::M_MS2SEC ); + current.pushVelocity.SubVec3(0) += (body->current->worldOrigin - body->saved.worldOrigin) / (deltaTime * idMath::M_MS2SEC); + current.pushVelocity.SubVec3(1) += rotation.GetVec() * -DEG2RAD(rotation.GetAngle()) / (deltaTime * idMath::M_MS2SEC); } } @@ -7838,7 +7889,7 @@ void idPhysics_AF::SetPushed( int deltaTime ) { idPhysics_AF::GetPushedLinearVelocity ================ */ -const idVec3 &idPhysics_AF::GetPushedLinearVelocity( const int id ) const { +const idVec3& idPhysics_AF::GetPushedLinearVelocity(const int id) const { return current.pushVelocity.SubVec3(0); } @@ -7847,7 +7898,7 @@ const idVec3 &idPhysics_AF::GetPushedLinearVelocity( const int id ) const { idPhysics_AF::GetPushedAngularVelocity ================ */ -const idVec3 &idPhysics_AF::GetPushedAngularVelocity( const int id ) const { +const idVec3& idPhysics_AF::GetPushedAngularVelocity(const int id) const { return current.pushVelocity.SubVec3(1); } @@ -7858,22 +7909,22 @@ idPhysics_AF::SetMaster the binding is orientated based on the constraints being used ================ */ -void idPhysics_AF::SetMaster( idEntity *master, const bool orientated ) { +void idPhysics_AF::SetMaster(idEntity* master, const bool orientated) { int i; idVec3 masterOrigin; idMat3 masterAxis; idRotation rotation; - if ( master ) { - self->GetMasterPosition( masterOrigin, masterAxis ); - if ( !masterBody ) { + if (master) { + self->GetMasterPosition(masterOrigin, masterAxis); + if (!masterBody) { masterBody = new idAFBody(); // translate and rotate all the constraints with body2 == NULL from world space to master space rotation = masterAxis.Transpose().ToRotation(); - for ( i = 0; i < constraints.Num(); i++ ) { - if ( constraints[i]->GetBody2() == NULL ) { - constraints[i]->Translate( -masterOrigin ); - constraints[i]->Rotate( rotation ); + for (i = 0; i < constraints.Num(); i++) { + if (constraints[i]->GetBody2() == NULL) { + constraints[i]->Translate(-masterOrigin); + constraints[i]->Rotate(rotation); } } Activate(); @@ -7882,13 +7933,13 @@ void idPhysics_AF::SetMaster( idEntity *master, const bool orientated ) { masterBody->current->worldAxis = masterAxis; } else { - if ( masterBody ) { + if (masterBody) { // translate and rotate all the constraints with body2 == NULL from master space to world space rotation = masterBody->current->worldAxis.ToRotation(); - for ( i = 0; i < constraints.Num(); i++ ) { - if ( constraints[i]->GetBody2() == NULL ) { - constraints[i]->Rotate( rotation ); - constraints[i]->Translate( masterBody->current->worldOrigin ); + for (i = 0; i < constraints.Num(); i++) { + if (constraints[i]->GetBody2() == NULL) { + constraints[i]->Rotate(rotation); + constraints[i]->Translate(masterBody->current->worldOrigin); } } delete masterBody; @@ -7899,59 +7950,59 @@ void idPhysics_AF::SetMaster( idEntity *master, const bool orientated ) { } -const float AF_VELOCITY_MAX = 16000; -const int AF_VELOCITY_TOTAL_BITS = 16; -const int AF_VELOCITY_EXPONENT_BITS = idMath::BitsForInteger( idMath::BitsForFloat( AF_VELOCITY_MAX ) ) + 1; -const int AF_VELOCITY_MANTISSA_BITS = AF_VELOCITY_TOTAL_BITS - 1 - AF_VELOCITY_EXPONENT_BITS; -const float AF_FORCE_MAX = 1e20f; -const int AF_FORCE_TOTAL_BITS = 16; -const int AF_FORCE_EXPONENT_BITS = idMath::BitsForInteger( idMath::BitsForFloat( AF_FORCE_MAX ) ) + 1; -const int AF_FORCE_MANTISSA_BITS = AF_FORCE_TOTAL_BITS - 1 - AF_FORCE_EXPONENT_BITS; +const float AF_VELOCITY_MAX = 16000; +const int AF_VELOCITY_TOTAL_BITS = 16; +const int AF_VELOCITY_EXPONENT_BITS = idMath::BitsForInteger(idMath::BitsForFloat(AF_VELOCITY_MAX)) + 1; +const int AF_VELOCITY_MANTISSA_BITS = AF_VELOCITY_TOTAL_BITS - 1 - AF_VELOCITY_EXPONENT_BITS; +const float AF_FORCE_MAX = 1e20f; +const int AF_FORCE_TOTAL_BITS = 16; +const int AF_FORCE_EXPONENT_BITS = idMath::BitsForInteger(idMath::BitsForFloat(AF_FORCE_MAX)) + 1; +const int AF_FORCE_MANTISSA_BITS = AF_FORCE_TOTAL_BITS - 1 - AF_FORCE_EXPONENT_BITS; /* ================ idPhysics_AF::WriteToSnapshot ================ */ -void idPhysics_AF::WriteToSnapshot( idBitMsgDelta &msg ) const { +void idPhysics_AF::WriteToSnapshot(idBitMsgDelta& msg) const { int i; idCQuat quat; - msg.WriteLong( current.atRest ); - msg.WriteFloat( current.noMoveTime ); - msg.WriteFloat( current.activateTime ); - msg.WriteDeltaFloat( 0.0f, current.pushVelocity[0], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, current.pushVelocity[1], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, current.pushVelocity[2], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, current.pushVelocity[3], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, current.pushVelocity[4], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, current.pushVelocity[5], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); + msg.WriteLong(current.atRest); + msg.WriteFloat(current.noMoveTime); + msg.WriteFloat(current.activateTime); + msg.WriteDeltaFloat(0.0f, current.pushVelocity[0], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, current.pushVelocity[1], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, current.pushVelocity[2], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, current.pushVelocity[3], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, current.pushVelocity[4], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, current.pushVelocity[5], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); - msg.WriteByte( bodies.Num() ); + msg.WriteByte(bodies.Num()); - for ( i = 0; i < bodies.Num(); i++ ) { - AFBodyPState_t *state = bodies[i]->current; + for (i = 0; i < bodies.Num(); i++) { + AFBodyPState_t* state = bodies[i]->current; quat = state->worldAxis.ToCQuat(); - msg.WriteFloat( state->worldOrigin[0] ); - msg.WriteFloat( state->worldOrigin[1] ); - msg.WriteFloat( state->worldOrigin[2] ); - msg.WriteFloat( quat.x ); - msg.WriteFloat( quat.y ); - msg.WriteFloat( quat.z ); - msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[0], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[1], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[2], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[3], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[4], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, state->spatialVelocity[5], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); -/* msg.WriteDeltaFloat( 0.0f, state->externalForce[0], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, state->externalForce[1], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, state->externalForce[2], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, state->externalForce[3], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, state->externalForce[4], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, state->externalForce[5], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); -*/ + msg.WriteFloat(state->worldOrigin[0]); + msg.WriteFloat(state->worldOrigin[1]); + msg.WriteFloat(state->worldOrigin[2]); + msg.WriteFloat(quat.x); + msg.WriteFloat(quat.y); + msg.WriteFloat(quat.z); + msg.WriteDeltaFloat(0.0f, state->spatialVelocity[0], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, state->spatialVelocity[1], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, state->spatialVelocity[2], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, state->spatialVelocity[3], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, state->spatialVelocity[4], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, state->spatialVelocity[5], AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + /* msg.WriteDeltaFloat( 0.0f, state->externalForce[0], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); + msg.WriteDeltaFloat( 0.0f, state->externalForce[1], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); + msg.WriteDeltaFloat( 0.0f, state->externalForce[2], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); + msg.WriteDeltaFloat( 0.0f, state->externalForce[3], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); + msg.WriteDeltaFloat( 0.0f, state->externalForce[4], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); + msg.WriteDeltaFloat( 0.0f, state->externalForce[5], AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); + */ } } @@ -7960,25 +8011,25 @@ void idPhysics_AF::WriteToSnapshot( idBitMsgDelta &msg ) const { idPhysics_AF::ReadFromSnapshot ================ */ -void idPhysics_AF::ReadFromSnapshot( const idBitMsgDelta &msg ) { +void idPhysics_AF::ReadFromSnapshot(const idBitMsgDelta& msg) { int i, num; idCQuat quat; current.atRest = msg.ReadLong(); current.noMoveTime = msg.ReadFloat(); current.activateTime = msg.ReadFloat(); - current.pushVelocity[0] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - current.pushVelocity[1] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - current.pushVelocity[2] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - current.pushVelocity[3] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - current.pushVelocity[4] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - current.pushVelocity[5] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); + current.pushVelocity[0] = msg.ReadDeltaFloat(0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + current.pushVelocity[1] = msg.ReadDeltaFloat(0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + current.pushVelocity[2] = msg.ReadDeltaFloat(0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + current.pushVelocity[3] = msg.ReadDeltaFloat(0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + current.pushVelocity[4] = msg.ReadDeltaFloat(0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + current.pushVelocity[5] = msg.ReadDeltaFloat(0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); num = msg.ReadByte(); - assert( num == bodies.Num() ); + assert(num == bodies.Num()); - for ( i = 0; i < bodies.Num(); i++ ) { - AFBodyPState_t *state = bodies[i]->current; + for (i = 0; i < bodies.Num(); i++) { + AFBodyPState_t* state = bodies[i]->current; state->worldOrigin[0] = msg.ReadFloat(); state->worldOrigin[1] = msg.ReadFloat(); @@ -7986,19 +8037,19 @@ void idPhysics_AF::ReadFromSnapshot( const idBitMsgDelta &msg ) { quat.x = msg.ReadFloat(); quat.y = msg.ReadFloat(); quat.z = msg.ReadFloat(); - state->spatialVelocity[0] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - state->spatialVelocity[1] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - state->spatialVelocity[2] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - state->spatialVelocity[3] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - state->spatialVelocity[4] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); - state->spatialVelocity[5] = msg.ReadDeltaFloat( 0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS ); -/* state->externalForce[0] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); - state->externalForce[1] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); - state->externalForce[2] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); - state->externalForce[3] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); - state->externalForce[4] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); - state->externalForce[5] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); -*/ + state->spatialVelocity[0] = msg.ReadDeltaFloat(0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + state->spatialVelocity[1] = msg.ReadDeltaFloat(0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + state->spatialVelocity[2] = msg.ReadDeltaFloat(0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + state->spatialVelocity[3] = msg.ReadDeltaFloat(0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + state->spatialVelocity[4] = msg.ReadDeltaFloat(0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + state->spatialVelocity[5] = msg.ReadDeltaFloat(0.0f, AF_VELOCITY_EXPONENT_BITS, AF_VELOCITY_MANTISSA_BITS); + /* state->externalForce[0] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); + state->externalForce[1] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); + state->externalForce[2] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); + state->externalForce[3] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); + state->externalForce[4] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); + state->externalForce[5] = msg.ReadDeltaFloat( 0.0f, AF_FORCE_EXPONENT_BITS, AF_FORCE_MANTISSA_BITS ); + */ state->worldAxis = quat.ToMat3(); } diff --git a/neo/game/physics/Physics_Monster.cpp b/neo/game/physics/Physics_Monster.cpp index 53a0f410..c4eabbff 100644 --- a/neo/game/physics/Physics_Monster.cpp +++ b/neo/game/physics/Physics_Monster.cpp @@ -2,9 +2,9 @@ =========================================================================== Doom 3 GPL Source Code -Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. +Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. -This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?). +This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?). Doom 3 Source Code is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -31,38 +31,76 @@ If you have questions concerning this license or the applicable additional terms #include "../Game_local.h" -CLASS_DECLARATION( idPhysics_Actor, idPhysics_Monster ) +CLASS_DECLARATION(idPhysics_Actor, idPhysics_Monster) END_CLASS const float OVERCLIP = 1.001f; + +// Doom 3 keeps monster movement as delta-based StepMove/SlideMove calls, while +// the newer idTech code moved to a frame-time based SlideMove. These constants +// let us port the low-risk parts of the newer clipping/step behaviour without +// changing the public Physics_Monster interface. +const int MONSTER_MAX_CLIP_PLANES = 5; +const int MONSTER_MAX_BUMPS = 4; +const float MONSTER_STEP_DOWN_EPSILON = 1.0e-4f; + /* ===================== idPhysics_Monster::CheckGround ===================== */ -void idPhysics_Monster::CheckGround( monsterPState_t &state ) { +void idPhysics_Monster::CheckGround(monsterPState_t& state) { + int i; trace_t groundTrace; idVec3 down; - if ( gravityNormal == vec3_zero ) { + if (gravityNormal == vec3_zero) { state.onGround = false; groundEntityPtr = NULL; return; } - down = state.origin + gravityNormal * CONTACT_EPSILON; - gameLocal.clip.Translation( groundTrace, state.origin, down, clipModel, clipModel->GetAxis(), clipMask, self ); + // The newer physics code builds the ground plane from actual contacts instead + // of a tiny downward trace. This is less sensitive to CONTACT_EPSILON and + // better represents monsters standing on multiple triangles or clipped movers. + clipModel->SetPosition(state.origin, clipModel->GetAxis()); + EvaluateContacts(); - if ( groundTrace.fraction == 1.0f ) { + groundTrace.endpos = state.origin; + groundTrace.endAxis = clipModel->GetAxis(); + if (contacts.Num()) { + groundTrace.fraction = 0.0f; + groundTrace.c = contacts[0]; + for (i = 1; i < contacts.Num(); i++) { + groundTrace.c.normal += contacts[i].normal; + } + groundTrace.c.normal.Normalize(); + } + else { + // Preserve Doom 3's tiny downward trace as a fallback when no contact was + // generated for the current position. + down = state.origin + gravityNormal * CONTACT_EPSILON; + gameLocal.clip.Translation(groundTrace, state.origin, down, clipModel, clipModel->GetAxis(), clipMask, self); + } + + // If neither contacts nor the fallback trace found anything, we are in free fall. + if (groundTrace.fraction == 1.0f || groundTrace.c.entityNum == ENTITYNUM_NONE) { state.onGround = false; groundEntityPtr = NULL; return; } - groundEntityPtr = gameLocal.entities[ groundTrace.c.entityNum ]; + groundEntityPtr = gameLocal.entities[groundTrace.c.entityNum]; - if ( ( groundTrace.c.normal * -gravityNormal ) < minFloorCosine ) { + // Match the newer kick-off check so upward velocity can detach the monster + // from the ground instead of keeping it artificially grounded. + if ((state.velocity * -gravityNormal) > 0.0f && (state.velocity * groundTrace.c.normal) > 10.0f) { + state.onGround = false; + return; + } + + if ((groundTrace.c.normal * -gravityNormal) < minFloorCosine) { state.onGround = false; return; } @@ -70,14 +108,14 @@ void idPhysics_Monster::CheckGround( monsterPState_t &state ) { state.onGround = true; // let the entity know about the collision - self->Collide( groundTrace, state.velocity ); + self->Collide(groundTrace, state.velocity); // apply impact to a non world floor entity - if ( groundTrace.c.entityNum != ENTITYNUM_WORLD && groundEntityPtr.GetEntity() ) { + if (groundTrace.c.entityNum != ENTITYNUM_WORLD && groundEntityPtr.GetEntity()) { impactInfo_t info; - groundEntityPtr.GetEntity()->GetImpactInfo( self, groundTrace.c.id, groundTrace.c.point, &info ); - if ( info.invMass != 0.0f ) { - groundEntityPtr.GetEntity()->ApplyImpulse( self, 0, groundTrace.c.point, state.velocity / ( info.invMass * 10.0f ) ); + groundEntityPtr.GetEntity()->GetImpactInfo(self, groundTrace.c.id, groundTrace.c.point, &info); + if (info.invMass != 0.0f) { + groundEntityPtr.GetEntity()->ApplyImpulse(self, 0, groundTrace.c.point, state.velocity / (info.invMass * 10.0f)); } } } @@ -87,32 +125,132 @@ void idPhysics_Monster::CheckGround( monsterPState_t &state ) { idPhysics_Monster::SlideMove ===================== */ -monsterMoveResult_t idPhysics_Monster::SlideMove( idVec3 &start, idVec3 &velocity, const idVec3 &delta ) { - int i; +monsterMoveResult_t idPhysics_Monster::SlideMove(idVec3& start, idVec3& velocity, const idVec3& delta) { + int i, j, k; + int bumpcount, numplanes; + float d, into; trace_t tr; - idVec3 move; + idVec3 move, remainingMove, clipVelocity, dir; + idVec3 planes[MONSTER_MAX_CLIP_PLANES]; + bool clipped; blockingEntity = NULL; + move = delta; - for( i = 0; i < 3; i++ ) { - gameLocal.clip.Translation( tr, start, start + move, clipModel, clipModel->GetAxis(), clipMask, self ); + if (move.LengthSqr() <= idMath::FLT_EPSILON) { + return MM_OK; + } + + clipped = false; + numplanes = 0; + + // Never turn back into the original velocity direction. This mirrors the + // newer multi-plane SlideMove logic but keeps Doom 3's delta-based interface. + if (velocity.LengthSqr() > idMath::FLT_EPSILON) { + planes[numplanes] = velocity; + planes[numplanes].Normalize(); + numplanes++; + } + + for (bumpcount = 0; bumpcount < MONSTER_MAX_BUMPS; bumpcount++) { + gameLocal.clip.Translation(tr, start, start + move, clipModel, clipModel->GetAxis(), clipMask, self); start = tr.endpos; - if ( tr.fraction == 1.0f ) { - if ( i > 0 ) { - return MM_SLIDING; - } - return MM_OK; + if (tr.fraction >= 1.0f) { + return clipped ? MM_SLIDING : MM_OK; } - if ( tr.c.entityNum != ENTITYNUM_NONE ) { - blockingEntity = gameLocal.entities[ tr.c.entityNum ]; - } - - // clip the movement delta and velocity - move.ProjectOntoPlane( tr.c.normal, OVERCLIP ); - velocity.ProjectOntoPlane( tr.c.normal, OVERCLIP ); + clipped = true; + + if (tr.c.entityNum != ENTITYNUM_NONE) { + blockingEntity = gameLocal.entities[tr.c.entityNum]; + } + + remainingMove = move * (1.0f - tr.fraction); + + if (numplanes >= MONSTER_MAX_CLIP_PLANES) { + // Same fail-safe as the newer code: high-poly/slanted collision soup can + // exhaust the available clip planes, so stop instead of oscillating. + velocity = vec3_origin; + return MM_BLOCKED; + } + + // If this is effectively the same plane we hit before, clip into the real + // trace normal and nudge velocity out along it. This fixes epsilon issues + // with non-axial planes that can otherwise leave the monster stuck. + for (i = 0; i < numplanes; i++) { + if ((tr.c.normal * planes[i]) > 0.999f) { + velocity.ProjectOntoPlane(tr.c.normal, OVERCLIP); + velocity += tr.c.normal; + remainingMove.ProjectOntoPlane(tr.c.normal, OVERCLIP); + break; + } + } + if (i < numplanes) { + move = remainingMove; + continue; + } + + planes[numplanes] = tr.c.normal; + numplanes++; + + // Modify velocity so it parallels all of the clip planes. This is the + // newer multi-plane clipping behaviour adapted to the old monster API. + for (i = 0; i < numplanes; i++) { + into = velocity * planes[i]; + if (into >= 0.1f) { + continue; + } + + clipVelocity = velocity; + clipVelocity.ProjectOntoPlane(planes[i], OVERCLIP); + + for (j = 0; j < numplanes; j++) { + if (j == i) { + continue; + } + if ((clipVelocity * planes[j]) >= 0.1f) { + continue; + } + + clipVelocity.ProjectOntoPlane(planes[j], OVERCLIP); + + if ((clipVelocity * planes[i]) >= 0.0f) { + continue; + } + + dir = planes[i].Cross(planes[j]); + dir.Normalize(); + d = dir * velocity; + clipVelocity = d * dir; + + for (k = 0; k < numplanes; k++) { + if (k == i || k == j) { + continue; + } + if ((clipVelocity * planes[k]) >= 0.1f) { + continue; + } + + velocity = vec3_origin; + return MM_BLOCKED; + } + } + + velocity = clipVelocity; + break; + } + + // Keep the remaining movement delta consistent with the newly collected + // planes. Doom 3's original code reused the full projected move, which can + // overshoot after a partial trace; use only the leftover distance. + for (i = 0; i < numplanes; i++) { + if ((remainingMove * planes[i]) < 0.1f) { + remainingMove.ProjectOntoPlane(planes[i], OVERCLIP); + } + } + move = remainingMove; } return MM_BLOCKED; @@ -126,57 +264,63 @@ idPhysics_Monster::StepMove the velocity is clipped conform any collisions ===================== */ -monsterMoveResult_t idPhysics_Monster::StepMove( idVec3 &start, idVec3 &velocity, const idVec3 &delta ) { +monsterMoveResult_t idPhysics_Monster::StepMove(idVec3& start, idVec3& velocity, const idVec3& delta) { trace_t tr; idVec3 up, down, noStepPos, noStepVel, stepPos, stepVel; monsterMoveResult_t result1, result2; float stepdist; float nostepdist; - if ( delta == vec3_origin ) { + if (delta == vec3_origin) { return MM_OK; } // try to move without stepping up noStepPos = start; noStepVel = velocity; - result1 = SlideMove( noStepPos, noStepVel, delta ); - if ( result1 == MM_OK ) { + result1 = SlideMove(noStepPos, noStepVel, delta); + if (result1 == MM_OK) { velocity = noStepVel; - if ( gravityNormal == vec3_zero ) { + if (gravityNormal == vec3_zero) { start = noStepPos; return MM_OK; } // try to step down so that we walk down slopes and stairs at a normal rate down = noStepPos + gravityNormal * maxStepHeight; - gameLocal.clip.Translation( tr, noStepPos, down, clipModel, clipModel->GetAxis(), clipMask, self ); - if ( tr.fraction < 1.0f ) { + gameLocal.clip.Translation(tr, noStepPos, down, clipModel, clipModel->GetAxis(), clipMask, self); + if (tr.fraction > MONSTER_STEP_DOWN_EPSILON && tr.fraction < 1.0f && (tr.c.normal * -gravityNormal) >= minFloorCosine) { start = tr.endpos; return MM_STEPPED; - } else { + } + else { start = noStepPos; return MM_OK; } } - if ( blockingEntity && blockingEntity->IsType( idActor::Type ) ) { + if (blockingEntity && blockingEntity->IsType(idActor::Type)) { // try to step down in case walking into an actor while going down steps down = noStepPos + gravityNormal * maxStepHeight; - gameLocal.clip.Translation( tr, noStepPos, down, clipModel, clipModel->GetAxis(), clipMask, self ); - start = tr.endpos; + gameLocal.clip.Translation(tr, noStepPos, down, clipModel, clipModel->GetAxis(), clipMask, self); + if (tr.fraction > MONSTER_STEP_DOWN_EPSILON && tr.fraction < 1.0f && (tr.c.normal * -gravityNormal) >= minFloorCosine) { + start = tr.endpos; + } + else { + start = noStepPos; + } velocity = noStepVel; return MM_BLOCKED; } - if ( gravityNormal == vec3_zero ) { + if (gravityNormal == vec3_zero) { return result1; } // try to step up up = start - gravityNormal * maxStepHeight; - gameLocal.clip.Translation( tr, start, up, clipModel, clipModel->GetAxis(), clipMask, self ); - if ( tr.fraction == 0.0f ) { + gameLocal.clip.Translation(tr, start, up, clipModel, clipModel->GetAxis(), clipMask, self); + if (tr.fraction == 0.0f) { start = noStepPos; velocity = noStepVel; return result1; @@ -185,8 +329,8 @@ monsterMoveResult_t idPhysics_Monster::StepMove( idVec3 &start, idVec3 &velocity // try to move at the stepped up position stepPos = tr.endpos; stepVel = velocity; - result2 = SlideMove( stepPos, stepVel, delta ); - if ( result2 == MM_BLOCKED ) { + result2 = SlideMove(stepPos, stepVel, delta); + if (result2 == MM_BLOCKED) { start = noStepPos; velocity = noStepVel; return result1; @@ -194,13 +338,16 @@ monsterMoveResult_t idPhysics_Monster::StepMove( idVec3 &start, idVec3 &velocity // step down again down = stepPos + gravityNormal * maxStepHeight; - gameLocal.clip.Translation( tr, stepPos, down, clipModel, clipModel->GetAxis(), clipMask, self ); + gameLocal.clip.Translation(tr, stepPos, down, clipModel, clipModel->GetAxis(), clipMask, self); stepPos = tr.endpos; - // if the move is further without stepping up, or the slope is too steap, don't step up - nostepdist = ( noStepPos - start ).LengthSqr(); - stepdist = ( stepPos - start ).LengthSqr(); - if ( ( nostepdist >= stepdist ) || ( ( tr.c.normal * -gravityNormal ) < minFloorCosine ) ) { + // if the move is further without stepping up, or the slope is too steep, don't step up. + // The newer code accepts a full-fraction step-down trace here, which avoids + // rejecting a valid step just because there was no immediate floor contact at + // the end of the downward trace. + nostepdist = (noStepPos - start).LengthSqr(); + stepdist = (stepPos - start).LengthSqr(); + if ((nostepdist >= stepdist) || (tr.fraction < 1.0f && (tr.c.normal * -gravityNormal) < minFloorCosine)) { start = noStepPos; velocity = noStepVel; return MM_SLIDING; @@ -217,9 +364,9 @@ monsterMoveResult_t idPhysics_Monster::StepMove( idVec3 &start, idVec3 &velocity idPhysics_Monster::Activate ================ */ -void idPhysics_Monster::Activate( void ) { +void idPhysics_Monster::Activate(void) { current.atRest = -1; - self->BecomeActive( TH_PHYSICS ); + self->BecomeActive(TH_PHYSICS); } /* @@ -227,10 +374,10 @@ void idPhysics_Monster::Activate( void ) { idPhysics_Monster::Rest ================ */ -void idPhysics_Monster::Rest( void ) { +void idPhysics_Monster::Rest(void) { current.atRest = gameLocal.time; current.velocity.Zero(); - self->BecomeInactive( TH_PHYSICS ); + self->BecomeInactive(TH_PHYSICS); } /* @@ -238,7 +385,7 @@ void idPhysics_Monster::Rest( void ) { idPhysics_Monster::PutToRest ================ */ -void idPhysics_Monster::PutToRest( void ) { +void idPhysics_Monster::PutToRest(void) { Rest(); } @@ -247,12 +394,12 @@ void idPhysics_Monster::PutToRest( void ) { idPhysics_Monster::idPhysics_Monster ================ */ -idPhysics_Monster::idPhysics_Monster( void ) { +idPhysics_Monster::idPhysics_Monster(void) { - memset( ¤t, 0, sizeof( current ) ); + memset(¤t, 0, sizeof(current)); current.atRest = -1; saved = current; - + delta.Zero(); maxStepHeight = 18.0f; minFloorCosine = 0.7f; @@ -269,13 +416,13 @@ idPhysics_Monster::idPhysics_Monster( void ) { idPhysics_Monster_SavePState ================ */ -void idPhysics_Monster_SavePState( idSaveGame *savefile, const monsterPState_t &state ) { - savefile->WriteVec3( state.origin ); - savefile->WriteVec3( state.velocity ); - savefile->WriteVec3( state.localOrigin ); - savefile->WriteVec3( state.pushVelocity ); - savefile->WriteBool( state.onGround ); - savefile->WriteInt( state.atRest ); +void idPhysics_Monster_SavePState(idSaveGame* savefile, const monsterPState_t& state) { + savefile->WriteVec3(state.origin); + savefile->WriteVec3(state.velocity); + savefile->WriteVec3(state.localOrigin); + savefile->WriteVec3(state.pushVelocity); + savefile->WriteBool(state.onGround); + savefile->WriteInt(state.atRest); } /* @@ -283,13 +430,18 @@ void idPhysics_Monster_SavePState( idSaveGame *savefile, const monsterPState_t & idPhysics_Monster_RestorePState ================ */ -void idPhysics_Monster_RestorePState( idRestoreGame *savefile, monsterPState_t &state ) { - savefile->ReadVec3( state.origin ); - savefile->ReadVec3( state.velocity ); - savefile->ReadVec3( state.localOrigin ); - savefile->ReadVec3( state.pushVelocity ); - savefile->ReadBool( state.onGround ); - savefile->ReadInt( state.atRest ); +void idPhysics_Monster_RestorePState(idRestoreGame* savefile, monsterPState_t& state) { + savefile->ReadVec3(state.origin); + savefile->ReadVec3(state.velocity); + savefile->ReadVec3(state.localOrigin); + savefile->ReadVec3(state.pushVelocity); + savefile->ReadBool(state.onGround); + savefile->ReadInt(state.atRest); + + state.origin.FixDenormals(); + state.velocity.FixDenormals(); + state.localOrigin.FixDenormals(); + state.pushVelocity.FixDenormals(); } /* @@ -297,22 +449,22 @@ void idPhysics_Monster_RestorePState( idRestoreGame *savefile, monsterPState_t & idPhysics_Monster::Save ================ */ -void idPhysics_Monster::Save( idSaveGame *savefile ) const { +void idPhysics_Monster::Save(idSaveGame* savefile) const { - idPhysics_Monster_SavePState( savefile, current ); - idPhysics_Monster_SavePState( savefile, saved ); + idPhysics_Monster_SavePState(savefile, current); + idPhysics_Monster_SavePState(savefile, saved); - savefile->WriteFloat( maxStepHeight ); - savefile->WriteFloat( minFloorCosine ); - savefile->WriteVec3( delta ); + savefile->WriteFloat(maxStepHeight); + savefile->WriteFloat(minFloorCosine); + savefile->WriteVec3(delta); - savefile->WriteBool( forceDeltaMove ); - savefile->WriteBool( fly ); - savefile->WriteBool( useVelocityMove ); - savefile->WriteBool( noImpact ); - - savefile->WriteInt( (int)moveResult ); - savefile->WriteObject( blockingEntity ); + savefile->WriteBool(forceDeltaMove); + savefile->WriteBool(fly); + savefile->WriteBool(useVelocityMove); + savefile->WriteBool(noImpact); + + savefile->WriteInt((int)moveResult); + savefile->WriteObject(blockingEntity); } /* @@ -320,22 +472,22 @@ void idPhysics_Monster::Save( idSaveGame *savefile ) const { idPhysics_Monster::Restore ================ */ -void idPhysics_Monster::Restore( idRestoreGame *savefile ) { +void idPhysics_Monster::Restore(idRestoreGame* savefile) { - idPhysics_Monster_RestorePState( savefile, current ); - idPhysics_Monster_RestorePState( savefile, saved ); + idPhysics_Monster_RestorePState(savefile, current); + idPhysics_Monster_RestorePState(savefile, saved); - savefile->ReadFloat( maxStepHeight ); - savefile->ReadFloat( minFloorCosine ); - savefile->ReadVec3( delta ); + savefile->ReadFloat(maxStepHeight); + savefile->ReadFloat(minFloorCosine); + savefile->ReadVec3(delta); - savefile->ReadBool( forceDeltaMove ); - savefile->ReadBool( fly ); - savefile->ReadBool( useVelocityMove ); - savefile->ReadBool( noImpact ); + savefile->ReadBool(forceDeltaMove); + savefile->ReadBool(fly); + savefile->ReadBool(useVelocityMove); + savefile->ReadBool(noImpact); - savefile->ReadInt( (int &)moveResult ); - savefile->ReadObject( reinterpret_cast( blockingEntity ) ); + savefile->ReadInt((int&)moveResult); + savefile->ReadObject(reinterpret_cast(blockingEntity)); } /* @@ -343,9 +495,9 @@ void idPhysics_Monster::Restore( idRestoreGame *savefile ) { idPhysics_Monster::SetDelta ================ */ -void idPhysics_Monster::SetDelta( const idVec3 &d ) { +void idPhysics_Monster::SetDelta(const idVec3& d) { delta = d; - if ( delta != vec3_origin ) { + if (delta != vec3_origin) { Activate(); } } @@ -355,7 +507,7 @@ void idPhysics_Monster::SetDelta( const idVec3 &d ) { idPhysics_Monster::SetMaxStepHeight ================ */ -void idPhysics_Monster::SetMaxStepHeight( const float newMaxStepHeight ) { +void idPhysics_Monster::SetMaxStepHeight(const float newMaxStepHeight) { maxStepHeight = newMaxStepHeight; } @@ -364,7 +516,7 @@ void idPhysics_Monster::SetMaxStepHeight( const float newMaxStepHeight ) { idPhysics_Monster::GetMaxStepHeight ================ */ -float idPhysics_Monster::GetMaxStepHeight( void ) const { +float idPhysics_Monster::GetMaxStepHeight(void) const { return maxStepHeight; } @@ -373,7 +525,7 @@ float idPhysics_Monster::GetMaxStepHeight( void ) const { idPhysics_Monster::OnGround ================ */ -bool idPhysics_Monster::OnGround( void ) const { +bool idPhysics_Monster::OnGround(void) const { return current.onGround; } @@ -382,7 +534,7 @@ bool idPhysics_Monster::OnGround( void ) const { idPhysics_Monster::GetSlideMoveEntity ================ */ -idEntity *idPhysics_Monster::GetSlideMoveEntity( void ) const { +idEntity* idPhysics_Monster::GetSlideMoveEntity(void) const { return blockingEntity; } @@ -391,7 +543,7 @@ idEntity *idPhysics_Monster::GetSlideMoveEntity( void ) const { idPhysics_Monster::GetMoveResult ================ */ -monsterMoveResult_t idPhysics_Monster::GetMoveResult( void ) const { +monsterMoveResult_t idPhysics_Monster::GetMoveResult(void) const { return moveResult; } @@ -400,7 +552,7 @@ monsterMoveResult_t idPhysics_Monster::GetMoveResult( void ) const { idPhysics_Monster::ForceDeltaMove ================ */ -void idPhysics_Monster::ForceDeltaMove( bool force ) { +void idPhysics_Monster::ForceDeltaMove(bool force) { forceDeltaMove = force; } @@ -409,7 +561,7 @@ void idPhysics_Monster::ForceDeltaMove( bool force ) { idPhysics_Monster::UseFlyMove ================ */ -void idPhysics_Monster::UseFlyMove( bool force ) { +void idPhysics_Monster::UseFlyMove(bool force) { fly = force; } @@ -418,7 +570,7 @@ void idPhysics_Monster::UseFlyMove( bool force ) { idPhysics_Monster::UseVelocityMove ================ */ -void idPhysics_Monster::UseVelocityMove( bool force ) { +void idPhysics_Monster::UseVelocityMove(bool force) { useVelocityMove = force; } @@ -427,7 +579,7 @@ void idPhysics_Monster::UseVelocityMove( bool force ) { idPhysics_Monster::EnableImpact ================ */ -void idPhysics_Monster::EnableImpact( void ) { +void idPhysics_Monster::EnableImpact(void) { noImpact = false; } @@ -436,7 +588,7 @@ void idPhysics_Monster::EnableImpact( void ) { idPhysics_Monster::DisableImpact ================ */ -void idPhysics_Monster::DisableImpact( void ) { +void idPhysics_Monster::DisableImpact(void) { noImpact = true; } @@ -445,31 +597,48 @@ void idPhysics_Monster::DisableImpact( void ) { idPhysics_Monster::Evaluate ================ */ -bool idPhysics_Monster::Evaluate( int timeStepMSec, int endTimeMSec ) { +bool idPhysics_Monster::Evaluate(int timeStepMSec, int endTimeMSec) { idVec3 masterOrigin, oldOrigin; idMat3 masterAxis; float timeStep; - timeStep = MS2SEC( timeStepMSec ); + // Snap away tiny values before prediction/network deltas. This mirrors the + // newer code's denormal cleanup and avoids long-running physics slowdowns. + current.origin.FixDenormals(); + current.velocity.FixDenormals(); + current.localOrigin.FixDenormals(); + current.pushVelocity.FixDenormals(); + delta.FixDenormals(); + + timeStep = MS2SEC(timeStepMSec); moveResult = MM_OK; blockingEntity = NULL; oldOrigin = current.origin; // if bound to a master - if ( masterEntity ) { - self->GetMasterPosition( masterOrigin, masterAxis ); + if (masterEntity) { + self->GetMasterPosition(masterOrigin, masterAxis); current.origin = masterOrigin + current.localOrigin * masterAxis; - clipModel->Link( gameLocal.clip, self, 0, current.origin, clipModel->GetAxis() ); - current.velocity = ( current.origin - oldOrigin ) / timeStep; + clipModel->Link(gameLocal.clip, self, 0, current.origin, clipModel->GetAxis()); + if (timeStepMSec > 0) { + current.velocity = (current.origin - oldOrigin) / timeStep; + } + else { + current.velocity.Zero(); + } masterDeltaYaw = masterYaw; masterYaw = masterAxis[0].ToYaw(); masterDeltaYaw = masterYaw - masterDeltaYaw; - return true; + return (current.origin != oldOrigin); } // if the monster is at rest - if ( current.atRest >= 0 ) { + if (current.atRest >= 0) { + return false; + } + + if (timeStepMSec <= 0) { return false; } @@ -481,17 +650,18 @@ bool idPhysics_Monster::Evaluate( int timeStepMSec, int endTimeMSec ) { clipModel->Unlink(); // check if on the ground - idPhysics_Monster::CheckGround( current ); + idPhysics_Monster::CheckGround(current); // if not on the ground or moving upwards float upspeed; - if ( gravityNormal != vec3_zero ) { - upspeed = -( current.velocity * gravityNormal ); - } else { + if (gravityNormal != vec3_zero) { + upspeed = -(current.velocity * gravityNormal); + } + else { upspeed = current.velocity.z; } - if ( fly || ( !forceDeltaMove && ( !current.onGround || upspeed > 1.0f ) ) ) { - if ( upspeed < 0.0f ) { + if (fly || (!forceDeltaMove && (!current.onGround || upspeed > 1.0f))) { + if (upspeed < 0.0f) { moveResult = MM_FALLING; } else { @@ -499,33 +669,36 @@ bool idPhysics_Monster::Evaluate( int timeStepMSec, int endTimeMSec ) { moveResult = MM_OK; } delta = current.velocity * timeStep; - if ( delta != vec3_origin ) { - moveResult = idPhysics_Monster::SlideMove( current.origin, current.velocity, delta ); - delta.Zero(); + if (delta != vec3_origin) { + moveResult = idPhysics_Monster::SlideMove(current.origin, current.velocity, delta); + delta.Zero(); } - if ( !fly ) { + if (!fly) { current.velocity += gravityVector * timeStep; } - } else { - if ( useVelocityMove ) { + } + else { + if (useVelocityMove) { delta = current.velocity * timeStep; - } else { + } + else { current.velocity = delta / timeStep; } - current.velocity -= ( current.velocity * gravityNormal ) * gravityNormal; + current.velocity -= (current.velocity * gravityNormal) * gravityNormal; - if ( delta == vec3_origin ) { + if (delta == vec3_origin) { Rest(); - } else { + } + else { // try moving into the desired direction - moveResult = idPhysics_Monster::StepMove( current.origin, current.velocity, delta ); + moveResult = idPhysics_Monster::StepMove(current.origin, current.velocity, delta); delta.Zero(); } } - clipModel->Link( gameLocal.clip, self, 0, current.origin, clipModel->GetAxis() ); + clipModel->Link(gameLocal.clip, self, 0, current.origin, clipModel->GetAxis()); // get all the ground contacts EvaluateContacts(); @@ -534,12 +707,12 @@ bool idPhysics_Monster::Evaluate( int timeStepMSec, int endTimeMSec ) { current.velocity += current.pushVelocity; current.pushVelocity.Zero(); - if ( IsOutsideWorld() ) { - gameLocal.Warning( "clip model outside world bounds for entity '%s' at (%s)", self->name.c_str(), current.origin.ToString(0) ); + if (IsOutsideWorld()) { + gameLocal.Warning("clip model outside world bounds for entity '%s' at (%s)", self->name.c_str(), current.origin.ToString(0)); Rest(); } - return ( current.origin != oldOrigin ); + return (current.origin != oldOrigin); } /* @@ -547,7 +720,7 @@ bool idPhysics_Monster::Evaluate( int timeStepMSec, int endTimeMSec ) { idPhysics_Monster::UpdateTime ================ */ -void idPhysics_Monster::UpdateTime( int endTimeMSec ) { +void idPhysics_Monster::UpdateTime(int endTimeMSec) { } /* @@ -555,7 +728,7 @@ void idPhysics_Monster::UpdateTime( int endTimeMSec ) { idPhysics_Monster::GetTime ================ */ -int idPhysics_Monster::GetTime( void ) const { +int idPhysics_Monster::GetTime(void) const { return gameLocal.time; } @@ -564,7 +737,7 @@ int idPhysics_Monster::GetTime( void ) const { idPhysics_Monster::GetImpactInfo ================ */ -void idPhysics_Monster::GetImpactInfo( const int id, const idVec3 &point, impactInfo_t *info ) const { +void idPhysics_Monster::GetImpactInfo(const int id, const idVec3& point, impactInfo_t* info) const { info->invMass = invMass; info->invInertiaTensor.Zero(); info->position.Zero(); @@ -576,8 +749,8 @@ void idPhysics_Monster::GetImpactInfo( const int id, const idVec3 &point, impact idPhysics_Monster::ApplyImpulse ================ */ -void idPhysics_Monster::ApplyImpulse( const int id, const idVec3 &point, const idVec3 &impulse ) { - if ( noImpact ) { +void idPhysics_Monster::ApplyImpulse(const int id, const idVec3& point, const idVec3& impulse) { + if (noImpact) { return; } current.velocity += impulse * invMass; @@ -589,7 +762,7 @@ void idPhysics_Monster::ApplyImpulse( const int id, const idVec3 &point, const i idPhysics_Monster::IsAtRest ================ */ -bool idPhysics_Monster::IsAtRest( void ) const { +bool idPhysics_Monster::IsAtRest(void) const { return current.atRest >= 0; } @@ -598,7 +771,7 @@ bool idPhysics_Monster::IsAtRest( void ) const { idPhysics_Monster::GetRestStartTime ================ */ -int idPhysics_Monster::GetRestStartTime( void ) const { +int idPhysics_Monster::GetRestStartTime(void) const { return current.atRest; } @@ -607,7 +780,7 @@ int idPhysics_Monster::GetRestStartTime( void ) const { idPhysics_Monster::SaveState ================ */ -void idPhysics_Monster::SaveState( void ) { +void idPhysics_Monster::SaveState(void) { saved = current; } @@ -616,10 +789,10 @@ void idPhysics_Monster::SaveState( void ) { idPhysics_Monster::RestoreState ================ */ -void idPhysics_Monster::RestoreState( void ) { +void idPhysics_Monster::RestoreState(void) { current = saved; - clipModel->Link( gameLocal.clip, self, 0, current.origin, clipModel->GetAxis() ); + clipModel->Link(gameLocal.clip, self, 0, current.origin, clipModel->GetAxis()); EvaluateContacts(); } @@ -629,19 +802,19 @@ void idPhysics_Monster::RestoreState( void ) { idPhysics_Player::SetOrigin ================ */ -void idPhysics_Monster::SetOrigin( const idVec3 &newOrigin, int id ) { +void idPhysics_Monster::SetOrigin(const idVec3& newOrigin, int id) { idVec3 masterOrigin; idMat3 masterAxis; current.localOrigin = newOrigin; - if ( masterEntity ) { - self->GetMasterPosition( masterOrigin, masterAxis ); + if (masterEntity) { + self->GetMasterPosition(masterOrigin, masterAxis); current.origin = masterOrigin + newOrigin * masterAxis; } else { current.origin = newOrigin; } - clipModel->Link( gameLocal.clip, self, 0, newOrigin, clipModel->GetAxis() ); + clipModel->Link(gameLocal.clip, self, 0, current.origin, clipModel->GetAxis()); Activate(); } @@ -650,8 +823,8 @@ void idPhysics_Monster::SetOrigin( const idVec3 &newOrigin, int id ) { idPhysics_Player::SetAxis ================ */ -void idPhysics_Monster::SetAxis( const idMat3 &newAxis, int id ) { - clipModel->Link( gameLocal.clip, self, 0, clipModel->GetOrigin(), newAxis ); +void idPhysics_Monster::SetAxis(const idMat3& newAxis, int id) { + clipModel->Link(gameLocal.clip, self, 0, current.origin, newAxis); Activate(); } @@ -660,11 +833,11 @@ void idPhysics_Monster::SetAxis( const idMat3 &newAxis, int id ) { idPhysics_Monster::Translate ================ */ -void idPhysics_Monster::Translate( const idVec3 &translation, int id ) { +void idPhysics_Monster::Translate(const idVec3& translation, int id) { current.localOrigin += translation; current.origin += translation; - clipModel->Link( gameLocal.clip, self, 0, current.origin, clipModel->GetAxis() ); + clipModel->Link(gameLocal.clip, self, 0, current.origin, clipModel->GetAxis()); Activate(); } @@ -673,19 +846,19 @@ void idPhysics_Monster::Translate( const idVec3 &translation, int id ) { idPhysics_Monster::Rotate ================ */ -void idPhysics_Monster::Rotate( const idRotation &rotation, int id ) { +void idPhysics_Monster::Rotate(const idRotation& rotation, int id) { idVec3 masterOrigin; idMat3 masterAxis; current.origin *= rotation; - if ( masterEntity ) { - self->GetMasterPosition( masterOrigin, masterAxis ); - current.localOrigin = ( current.origin - masterOrigin ) * masterAxis.Transpose(); + if (masterEntity) { + self->GetMasterPosition(masterOrigin, masterAxis); + current.localOrigin = (current.origin - masterOrigin) * masterAxis.Transpose(); } else { current.localOrigin = current.origin; } - clipModel->Link( gameLocal.clip, self, 0, current.origin, clipModel->GetAxis() * rotation.ToMat3() ); + clipModel->Link(gameLocal.clip, self, 0, current.origin, clipModel->GetAxis() * rotation.ToMat3()); Activate(); } @@ -694,7 +867,7 @@ void idPhysics_Monster::Rotate( const idRotation &rotation, int id ) { idPhysics_Monster::SetLinearVelocity ================ */ -void idPhysics_Monster::SetLinearVelocity( const idVec3 &newLinearVelocity, int id ) { +void idPhysics_Monster::SetLinearVelocity(const idVec3& newLinearVelocity, int id) { current.velocity = newLinearVelocity; Activate(); } @@ -704,7 +877,7 @@ void idPhysics_Monster::SetLinearVelocity( const idVec3 &newLinearVelocity, int idPhysics_Monster::GetLinearVelocity ================ */ -const idVec3 &idPhysics_Monster::GetLinearVelocity( int id ) const { +const idVec3& idPhysics_Monster::GetLinearVelocity(int id) const { return current.velocity; } @@ -713,9 +886,13 @@ const idVec3 &idPhysics_Monster::GetLinearVelocity( int id ) const { idPhysics_Monster::SetPushed ================ */ -void idPhysics_Monster::SetPushed( int deltaTime ) { +void idPhysics_Monster::SetPushed(int deltaTime) { + if (deltaTime <= 0) { + return; + } + // velocity with which the monster is pushed - current.pushVelocity += ( current.origin - saved.origin ) / ( deltaTime * idMath::M_MS2SEC ); + current.pushVelocity += (current.origin - saved.origin) / (deltaTime * idMath::M_MS2SEC); } /* @@ -723,7 +900,7 @@ void idPhysics_Monster::SetPushed( int deltaTime ) { idPhysics_Monster::GetPushedLinearVelocity ================ */ -const idVec3 &idPhysics_Monster::GetPushedLinearVelocity( const int id ) const { +const idVec3& idPhysics_Monster::GetPushedLinearVelocity(const int id) const { return current.pushVelocity; } @@ -734,53 +911,53 @@ idPhysics_Monster::SetMaster the binding is never orientated ================ */ -void idPhysics_Monster::SetMaster( idEntity *master, const bool orientated ) { +void idPhysics_Monster::SetMaster(idEntity* master, const bool orientated) { idVec3 masterOrigin; idMat3 masterAxis; - if ( master ) { - if ( !masterEntity ) { + if (master) { + if (!masterEntity) { // transform from world space to master space - self->GetMasterPosition( masterOrigin, masterAxis ); - current.localOrigin = ( current.origin - masterOrigin ) * masterAxis.Transpose(); + self->GetMasterPosition(masterOrigin, masterAxis); + current.localOrigin = (current.origin - masterOrigin) * masterAxis.Transpose(); masterEntity = master; masterYaw = masterAxis[0].ToYaw(); } ClearContacts(); } else { - if ( masterEntity ) { + if (masterEntity) { masterEntity = NULL; Activate(); } } } -const float MONSTER_VELOCITY_MAX = 4000; -const int MONSTER_VELOCITY_TOTAL_BITS = 16; -const int MONSTER_VELOCITY_EXPONENT_BITS = idMath::BitsForInteger( idMath::BitsForFloat( MONSTER_VELOCITY_MAX ) ) + 1; -const int MONSTER_VELOCITY_MANTISSA_BITS = MONSTER_VELOCITY_TOTAL_BITS - 1 - MONSTER_VELOCITY_EXPONENT_BITS; +const float MONSTER_VELOCITY_MAX = 4000; +const int MONSTER_VELOCITY_TOTAL_BITS = 16; +const int MONSTER_VELOCITY_EXPONENT_BITS = idMath::BitsForInteger(idMath::BitsForFloat(MONSTER_VELOCITY_MAX)) + 1; +const int MONSTER_VELOCITY_MANTISSA_BITS = MONSTER_VELOCITY_TOTAL_BITS - 1 - MONSTER_VELOCITY_EXPONENT_BITS; /* ================ idPhysics_Monster::WriteToSnapshot ================ */ -void idPhysics_Monster::WriteToSnapshot( idBitMsgDelta &msg ) const { - msg.WriteFloat( current.origin[0] ); - msg.WriteFloat( current.origin[1] ); - msg.WriteFloat( current.origin[2] ); - msg.WriteFloat( current.velocity[0], MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS ); - msg.WriteFloat( current.velocity[1], MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS ); - msg.WriteFloat( current.velocity[2], MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( current.origin[0], current.localOrigin[0] ); - msg.WriteDeltaFloat( current.origin[1], current.localOrigin[1] ); - msg.WriteDeltaFloat( current.origin[2], current.localOrigin[2] ); - msg.WriteDeltaFloat( 0.0f, current.pushVelocity[0], MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, current.pushVelocity[1], MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, current.pushVelocity[2], MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS ); - msg.WriteLong( current.atRest ); - msg.WriteBits( current.onGround, 1 ); +void idPhysics_Monster::WriteToSnapshot(idBitMsgDelta& msg) const { + msg.WriteFloat(current.origin[0]); + msg.WriteFloat(current.origin[1]); + msg.WriteFloat(current.origin[2]); + msg.WriteFloat(current.velocity[0], MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS); + msg.WriteFloat(current.velocity[1], MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS); + msg.WriteFloat(current.velocity[2], MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(current.origin[0], current.localOrigin[0]); + msg.WriteDeltaFloat(current.origin[1], current.localOrigin[1]); + msg.WriteDeltaFloat(current.origin[2], current.localOrigin[2]); + msg.WriteDeltaFloat(0.0f, current.pushVelocity[0], MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, current.pushVelocity[1], MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, current.pushVelocity[2], MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS); + msg.WriteLong(current.atRest); + msg.WriteBits(current.onGround, 1); } /* @@ -788,19 +965,28 @@ void idPhysics_Monster::WriteToSnapshot( idBitMsgDelta &msg ) const { idPhysics_Monster::ReadFromSnapshot ================ */ -void idPhysics_Monster::ReadFromSnapshot( const idBitMsgDelta &msg ) { +void idPhysics_Monster::ReadFromSnapshot(const idBitMsgDelta& msg) { current.origin[0] = msg.ReadFloat(); current.origin[1] = msg.ReadFloat(); current.origin[2] = msg.ReadFloat(); - current.velocity[0] = msg.ReadFloat( MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS ); - current.velocity[1] = msg.ReadFloat( MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS ); - current.velocity[2] = msg.ReadFloat( MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS ); - current.localOrigin[0] = msg.ReadDeltaFloat( current.origin[0] ); - current.localOrigin[1] = msg.ReadDeltaFloat( current.origin[1] ); - current.localOrigin[2] = msg.ReadDeltaFloat( current.origin[2] ); - current.pushVelocity[0] = msg.ReadDeltaFloat( 0.0f, MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS ); - current.pushVelocity[1] = msg.ReadDeltaFloat( 0.0f, MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS ); - current.pushVelocity[2] = msg.ReadDeltaFloat( 0.0f, MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS ); + current.velocity[0] = msg.ReadFloat(MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS); + current.velocity[1] = msg.ReadFloat(MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS); + current.velocity[2] = msg.ReadFloat(MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS); + current.localOrigin[0] = msg.ReadDeltaFloat(current.origin[0]); + current.localOrigin[1] = msg.ReadDeltaFloat(current.origin[1]); + current.localOrigin[2] = msg.ReadDeltaFloat(current.origin[2]); + current.pushVelocity[0] = msg.ReadDeltaFloat(0.0f, MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS); + current.pushVelocity[1] = msg.ReadDeltaFloat(0.0f, MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS); + current.pushVelocity[2] = msg.ReadDeltaFloat(0.0f, MONSTER_VELOCITY_EXPONENT_BITS, MONSTER_VELOCITY_MANTISSA_BITS); current.atRest = msg.ReadLong(); - current.onGround = msg.ReadBits( 1 ) != 0; + current.onGround = msg.ReadBits(1) != 0; + + current.origin.FixDenormals(); + current.velocity.FixDenormals(); + current.localOrigin.FixDenormals(); + current.pushVelocity.FixDenormals(); + + if (clipModel) { + clipModel->Link(gameLocal.clip, self, 0, current.origin, clipModel->GetAxis()); + } } diff --git a/neo/game/physics/Physics_Player.cpp b/neo/game/physics/Physics_Player.cpp index 8a337cc1..91381b88 100644 --- a/neo/game/physics/Physics_Player.cpp +++ b/neo/game/physics/Physics_Player.cpp @@ -2,9 +2,9 @@ =========================================================================== Doom 3 GPL Source Code -Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. +Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. -This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?). +This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?). Doom 3 Source Code is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -31,42 +31,126 @@ If you have questions concerning this license or the applicable additional terms #include "../Game_local.h" -CLASS_DECLARATION( idPhysics_Actor, idPhysics_Player ) +CLASS_DECLARATION(idPhysics_Actor, idPhysics_Player) END_CLASS // movement parameters -const float PM_STOPSPEED = 100.0f; -const float PM_SWIMSCALE = 0.5f; -const float PM_LADDERSPEED = 100.0f; -const float PM_STEPSCALE = 1.0f; +const float PM_STOPSPEED = 100.0f; +const float PM_SWIMSCALE = 0.5f; +const float PM_LADDERSPEED = 100.0f; +const float PM_STEPSCALE = 1.0f; -const float PM_ACCELERATE = 10.0f; -const float PM_AIRACCELERATE = 1.0f; -const float PM_WATERACCELERATE = 4.0f; -const float PM_FLYACCELERATE = 8.0f; +const float PM_ACCELERATE = 10.0f; +const float PM_AIRACCELERATE = 1.0f; +const float PM_WATERACCELERATE = 4.0f; +const float PM_FLYACCELERATE = 8.0f; -const float PM_FRICTION = 6.0f; -const float PM_AIRFRICTION = 0.0f; -const float PM_WATERFRICTION = 1.0f; -const float PM_FLYFRICTION = 3.0f; -const float PM_NOCLIPFRICTION = 12.0f; +const float PM_FRICTION = 6.0f; +const float PM_AIRFRICTION = 0.2f; +const float PM_WATERFRICTION = 1.0f; +const float PM_FLYFRICTION = 6.0f; +const float PM_NOCLIPFRICTION = 12.0f; -const float MIN_WALK_NORMAL = 0.7f; // can't walk on very steep slopes -const float OVERCLIP = 1.001f; +const float MIN_WALK_NORMAL = 0.7f; // can't walk on very steep slopes +const float OVERCLIP = 1.001f; // movementFlags -const int PMF_DUCKED = 1; // set when ducking -const int PMF_JUMPED = 2; // set when the player jumped this frame -const int PMF_STEPPED_UP = 4; // set when the player stepped up this frame -const int PMF_STEPPED_DOWN = 8; // set when the player stepped down this frame -const int PMF_JUMP_HELD = 16; // set when jump button is held down -const int PMF_TIME_LAND = 32; // movementTime is time before rejump -const int PMF_TIME_KNOCKBACK = 64; // movementTime is an air-accelerate only time -const int PMF_TIME_WATERJUMP = 128; // movementTime is waterjump -const int PMF_ALL_TIMES = (PMF_TIME_WATERJUMP|PMF_TIME_LAND|PMF_TIME_KNOCKBACK); +const int PMF_DUCKED = 1; // set when ducking +const int PMF_JUMPED = 2; // set when the player jumped this frame +const int PMF_STEPPED_UP = 4; // set when the player stepped up this frame +const int PMF_STEPPED_DOWN = 8; // set when the player stepped down this frame +const int PMF_JUMP_HELD = 16; // set when jump button is held down +const int PMF_TIME_LAND = 32; // movementTime is time before rejump +const int PMF_TIME_KNOCKBACK = 64; // movementTime is an air-accelerate only time +const int PMF_TIME_WATERJUMP = 128; // movementTime is waterjump +const int PMF_ALL_TIMES = (PMF_TIME_WATERJUMP | PMF_TIME_LAND | PMF_TIME_KNOCKBACK); int c_pmove = 0; + +/* +================== +PM_CmdScale + +ETQW-style command scaling helper. The original Doom 3 CmdScale only +ignored upmove while walking; air movement also has no vertical control, +so jump/crouch input should not reduce horizontal air wishspeed. +================== +*/ +static float PM_CmdScale(const usercmd_t& cmd, const float playerSpeed, const bool noVertical) { + int max; + float total; + int forwardmove; + int rightmove; + int upmove; + + forwardmove = cmd.forwardmove; + rightmove = cmd.rightmove; + upmove = noVertical ? 0 : cmd.upmove; + + max = abs(forwardmove); + if (abs(rightmove) > max) { + max = abs(rightmove); + } + if (abs(upmove) > max) { + max = abs(upmove); + } + + if (!max) { + return 0.0f; + } + + total = idMath::Sqrt((float)forwardmove * forwardmove + rightmove * rightmove + upmove * upmove); + return (float)playerSpeed * max / (127.0f * total); +} + +/* +================== +PM_IsWalkablePlane +================== +*/ +static bool PM_IsWalkablePlane(const idVec3& normal, const idVec3& gravityNormal) { + return (normal * -gravityNormal) >= MIN_WALK_NORMAL; +} + +/* +================== +PM_AdjustVertically + +Given a walkable plane normal, adjust the vertical component so the vector lies +in the plane without changing the horizontal direction. This is the portable +part of ETQW's idPhysics_Player::AdjustVertically and replaces several slope +speed-loss cases caused by plain ProjectOntoPlane. +================== +*/ +static idVec3 PM_AdjustVertically(const idVec3& normal, const idVec3& _in, const idVec3& gravityNormal) { + idVec3 out; + idVec3 in = _in; + idVec3 up = -gravityNormal; + idVec3 velocityPlane = in.Cross(up); + + // same horizontal direction and in the walkable plane + out = velocityPlane.Cross(normal); + + // make sure direction is ok + if (in * out < 0.0f) { + out = -out; + } + + // maintain an overclip on the vertical component so rounding problems do not + // put us back into solids. For normal Doom 3 gravity this matches ETQW's z test. + float upComponent = out * up; + idVec3 vertical = up * upComponent; + idVec3 horizontal = out - vertical; + if (upComponent >= 0.0f) { + vertical *= OVERCLIP; + } + else { + vertical /= OVERCLIP; + } + return horizontal + vertical; +} + /* ============ idPhysics_Player::CmdScale @@ -76,41 +160,10 @@ This allows the clients to use axial -127 to 127 values for all directions without getting a sqrt(2) distortion in speed. ============ */ -float idPhysics_Player::CmdScale( const usercmd_t &cmd ) const { - int max; - float total; - float scale; - int forwardmove; - int rightmove; - int upmove; - - forwardmove = cmd.forwardmove; - rightmove = cmd.rightmove; - - // since the crouch key doubles as downward movement, ignore downward movement when we're on the ground - // otherwise crouch speed will be lower than specified - if ( walking ) { - upmove = 0; - } else { - upmove = cmd.upmove; - } - - max = abs( forwardmove ); - if ( abs( rightmove ) > max ) { - max = abs( rightmove ); - } - if ( abs( upmove ) > max ) { - max = abs( upmove ); - } - - if ( !max ) { - return 0.0f; - } - - total = idMath::Sqrt( (float) forwardmove * forwardmove + rightmove * rightmove + upmove * upmove ); - scale = (float) playerSpeed * max / ( 127.0f * total ); - - return scale; +float idPhysics_Player::CmdScale(const usercmd_t& cmd) const { + // Preserve Doom 3's public behavior for callers that do not opt in to the + // explicit noVertical path: walking ignores upmove, non-walking modes keep it. + return PM_CmdScale(cmd, playerSpeed, walking); } /* @@ -120,7 +173,7 @@ idPhysics_Player::Accelerate Handles user intended acceleration ============== */ -void idPhysics_Player::Accelerate( const idVec3 &wishdir, const float wishspeed, const float accel ) { +void idPhysics_Player::Accelerate(const idVec3& wishdir, const float wishspeed, const float accel) { #if 1 // q2 style float addspeed, accelspeed, currentspeed; @@ -134,7 +187,7 @@ void idPhysics_Player::Accelerate( const idVec3 &wishdir, const float wishspeed, if (accelspeed > addspeed) { accelspeed = addspeed; } - + current.velocity += accelspeed * wishdir; #else // proper way (avoids strafe jump maxspeed bug), but feels bad @@ -165,7 +218,7 @@ Returns true if the velocity was clipped in some way */ #define MAX_CLIP_PLANES 5 -bool idPhysics_Player::SlideMove( bool gravity, bool stepUp, bool stepDown, bool push ) { +bool idPhysics_Player::SlideMove(bool gravity, bool stepUp, bool stepDown, bool push) { int i, j, k, pushFlags; int bumpcount, numbumps, numplanes; float d, time_left, into, totalMass; @@ -178,13 +231,18 @@ bool idPhysics_Player::SlideMove( bool gravity, bool stepUp, bool stepDown, bool primal_velocity = current.velocity; - if ( gravity ) { + if (gravity) { endVelocity = current.velocity + gravityVector * frametime; - current.velocity = ( current.velocity + endVelocity ) * 0.5f; + current.velocity = (current.velocity + endVelocity) * 0.5f; primal_velocity = endVelocity; - if ( groundPlane ) { - // slide along the ground plane - current.velocity.ProjectOntoPlane( groundTrace.c.normal, OVERCLIP ); + if (groundPlane) { + // slide along the ground plane without losing horizontal speed on walkable slopes + if (PM_IsWalkablePlane(groundTrace.c.normal, gravityNormal)) { + current.velocity = PM_AdjustVertically(groundTrace.c.normal, current.velocity, gravityNormal); + } + else { + current.velocity.ProjectOntoPlane(groundTrace.c.normal, OVERCLIP); + } } } else { @@ -194,10 +252,11 @@ bool idPhysics_Player::SlideMove( bool gravity, bool stepUp, bool stepDown, bool time_left = frametime; // never turn against the ground plane - if ( groundPlane ) { + if (groundPlane) { numplanes = 1; planes[0] = groundTrace.c.normal; - } else { + } + else { numplanes = 0; } @@ -206,58 +265,58 @@ bool idPhysics_Player::SlideMove( bool gravity, bool stepUp, bool stepDown, bool planes[numplanes].Normalize(); numplanes++; - for ( bumpcount = 0; bumpcount < numbumps; bumpcount++ ) { + for (bumpcount = 0; bumpcount < numbumps; bumpcount++) { // calculate position we are trying to move to end = current.origin + time_left * current.velocity; // see if we can make it there - gameLocal.clip.Translation( trace, current.origin, end, clipModel, clipModel->GetAxis(), clipMask, self ); + gameLocal.clip.Translation(trace, current.origin, end, clipModel, clipModel->GetAxis(), clipMask, self); time_left -= time_left * trace.fraction; current.origin = trace.endpos; // if moved the entire distance - if ( trace.fraction >= 1.0f ) { + if (trace.fraction >= 1.0f) { break; } stepped = pushed = false; - // if we are allowed to step up - if ( stepUp ) { + // if we are allowed to step up, only do stair stepping for non-walkable hits + if (stepUp && (trace.c.normal * -gravityNormal) < MIN_WALK_NORMAL) { - nearGround = groundPlane | ladder; + nearGround = groundPlane || ladder; - if ( !nearGround ) { + if (!nearGround) { // trace down to see if the player is near the ground // step checking when near the ground allows the player to move up stairs smoothly while jumping stepEnd = current.origin + maxStepHeight * gravityNormal; - gameLocal.clip.Translation( downTrace, current.origin, stepEnd, clipModel, clipModel->GetAxis(), clipMask, self ); - nearGround = ( downTrace.fraction < 1.0f && (downTrace.c.normal * -gravityNormal) > MIN_WALK_NORMAL ); + gameLocal.clip.Translation(downTrace, current.origin, stepEnd, clipModel, clipModel->GetAxis(), clipMask, self); + nearGround = (downTrace.fraction < 1.0f && (downTrace.c.normal * -gravityNormal) > MIN_WALK_NORMAL); } // may only step up if near the ground or on a ladder - if ( nearGround ) { + if (nearGround) { // step up stepEnd = current.origin - maxStepHeight * gravityNormal; - gameLocal.clip.Translation( downTrace, current.origin, stepEnd, clipModel, clipModel->GetAxis(), clipMask, self ); + gameLocal.clip.Translation(downTrace, current.origin, stepEnd, clipModel, clipModel->GetAxis(), clipMask, self); // trace along velocity stepEnd = downTrace.endpos + time_left * current.velocity; - gameLocal.clip.Translation( stepTrace, downTrace.endpos, stepEnd, clipModel, clipModel->GetAxis(), clipMask, self ); + gameLocal.clip.Translation(stepTrace, downTrace.endpos, stepEnd, clipModel, clipModel->GetAxis(), clipMask, self); // step down stepEnd = stepTrace.endpos + maxStepHeight * gravityNormal; - gameLocal.clip.Translation( downTrace, stepTrace.endpos, stepEnd, clipModel, clipModel->GetAxis(), clipMask, self ); + gameLocal.clip.Translation(downTrace, stepTrace.endpos, stepEnd, clipModel, clipModel->GetAxis(), clipMask, self); - if ( downTrace.fraction >= 1.0f || (downTrace.c.normal * -gravityNormal) > MIN_WALK_NORMAL ) { + if (downTrace.fraction >= 1.0f || (downTrace.c.normal * -gravityNormal) > MIN_WALK_NORMAL) { // if moved the entire distance - if ( stepTrace.fraction >= 1.0f ) { + if (stepTrace.fraction >= 1.0f) { time_left = 0; - current.stepUp -= ( downTrace.endpos - current.origin ) * gravityNormal; + current.stepUp -= (downTrace.endpos - current.origin) * gravityNormal; current.origin = downTrace.endpos; current.movementFlags |= PMF_STEPPED_UP; current.velocity *= PM_STEPSCALE; @@ -265,9 +324,9 @@ bool idPhysics_Player::SlideMove( bool gravity, bool stepUp, bool stepDown, bool } // if the move is further when stepping up - if ( stepTrace.fraction > trace.fraction ) { + if (stepTrace.fraction > trace.fraction) { time_left -= time_left * stepTrace.fraction; - current.stepUp -= ( downTrace.endpos - current.origin ) * gravityNormal; + current.stepUp -= (downTrace.endpos - current.origin) * gravityNormal; current.origin = downTrace.endpos; current.movementFlags |= PMF_STEPPED_UP; current.velocity *= PM_STEPSCALE; @@ -279,38 +338,38 @@ bool idPhysics_Player::SlideMove( bool gravity, bool stepUp, bool stepDown, bool } // if we can push other entities and not blocked by the world - if ( push && trace.c.entityNum != ENTITYNUM_WORLD ) { + if (push && trace.c.entityNum != ENTITYNUM_WORLD) { - clipModel->SetPosition( current.origin, clipModel->GetAxis() ); + clipModel->SetPosition(current.origin, clipModel->GetAxis()); // clip movement, only push idMoveables, don't push entities the player is standing on // apply impact to pushed objects - pushFlags = PUSHFL_CLIP|PUSHFL_ONLYMOVEABLE|PUSHFL_NOGROUNDENTITIES|PUSHFL_APPLYIMPULSE; + pushFlags = PUSHFL_CLIP | PUSHFL_ONLYMOVEABLE | PUSHFL_NOGROUNDENTITIES | PUSHFL_APPLYIMPULSE; // clip & push - totalMass = gameLocal.push.ClipTranslationalPush( trace, self, pushFlags, end, end - current.origin ); + totalMass = gameLocal.push.ClipTranslationalPush(trace, self, pushFlags, end, end - current.origin); - if ( totalMass > 0.0f ) { + if (totalMass > 0.0f) { // decrease velocity based on the total mass of the objects being pushed ? - current.velocity *= 1.0f - idMath::ClampFloat( 0.0f, 1000.0f, totalMass - 20.0f ) * ( 1.0f / 950.0f ); + current.velocity *= 1.0f - idMath::ClampFloat(0.0f, 1000.0f, totalMass - 20.0f) * (1.0f / 950.0f); pushed = true; } - + current.origin = trace.endpos; time_left -= time_left * trace.fraction; // if moved the entire distance - if ( trace.fraction >= 1.0f ) { + if (trace.fraction >= 1.0f) { break; } } - if ( !stepped ) { + if (!stepped) { // let the entity know about the collision - self->Collide( trace, current.velocity ); + self->Collide(trace, current.velocity); } - if ( numplanes >= MAX_CLIP_PLANES ) { + if (numplanes >= MAX_CLIP_PLANES) { // MrElusive: I think we have some relatively high poly LWO models with a lot of slanted tris // where it may hit the max clip planes current.velocity = vec3_origin; @@ -322,13 +381,21 @@ bool idPhysics_Player::SlideMove( bool gravity, bool stepUp, bool stepDown, bool // out along it, which fixes some epsilon issues with // non-axial planes // - for ( i = 0; i < numplanes; i++ ) { - if ( ( trace.c.normal * planes[i] ) > 0.999f ) { - current.velocity += trace.c.normal; + for (i = 0; i < numplanes; i++) { + if ((trace.c.normal * planes[i]) > 0.999f) { + if (PM_IsWalkablePlane(trace.c.normal, gravityNormal)) { + current.velocity = PM_AdjustVertically(trace.c.normal, current.velocity, gravityNormal); + } + else { + // clip into the trace normal just in case this normal is almost but not exactly + // the same as the groundTrace normal, then nudge out along the normal. + current.velocity.ProjectOntoPlane(trace.c.normal, OVERCLIP); + current.velocity += trace.c.normal; + } break; } } - if ( i < numplanes ) { + if (i < numplanes) { continue; } planes[numplanes] = trace.c.normal; @@ -339,55 +406,67 @@ bool idPhysics_Player::SlideMove( bool gravity, bool stepUp, bool stepDown, bool // // find a plane that it enters - for ( i = 0; i < numplanes; i++ ) { + for (i = 0; i < numplanes; i++) { into = current.velocity * planes[i]; - if ( into >= 0.1f ) { + if (into >= 0.1f) { continue; // move doesn't interact with the plane } - // slide along the plane - clipVelocity = current.velocity; - clipVelocity.ProjectOntoPlane( planes[i], OVERCLIP ); + if (PM_IsWalkablePlane(planes[i], gravityNormal)) { + clipVelocity = PM_AdjustVertically(planes[i], current.velocity, gravityNormal); + endClipVelocity = PM_AdjustVertically(planes[i], endVelocity, gravityNormal); + } + else { + // slide along the plane + clipVelocity = current.velocity; + clipVelocity.ProjectOntoPlane(planes[i], OVERCLIP); - // slide along the plane - endClipVelocity = endVelocity; - endClipVelocity.ProjectOntoPlane( planes[i], OVERCLIP ); + // slide along the plane + endClipVelocity = endVelocity; + endClipVelocity.ProjectOntoPlane(planes[i], OVERCLIP); + } // see if there is a second plane that the new move enters - for ( j = 0; j < numplanes; j++ ) { - if ( j == i ) { + for (j = 0; j < numplanes; j++) { + if (j == i) { continue; } - if ( ( clipVelocity * planes[j] ) >= 0.1f ) { + if ((clipVelocity * planes[j]) >= 0.1f) { continue; // move doesn't interact with the plane } - // try clipping the move to the plane - clipVelocity.ProjectOntoPlane( planes[j], OVERCLIP ); - endClipVelocity.ProjectOntoPlane( planes[j], OVERCLIP ); + if (PM_IsWalkablePlane(planes[j], gravityNormal)) { + clipVelocity = PM_AdjustVertically(planes[j], clipVelocity, gravityNormal); + endClipVelocity = PM_AdjustVertically(planes[j], endClipVelocity, gravityNormal); + } + else { + // try clipping the move to the plane + clipVelocity.ProjectOntoPlane(planes[j], OVERCLIP); + endClipVelocity.ProjectOntoPlane(planes[j], OVERCLIP); + } // see if it goes back into the first clip plane - if ( ( clipVelocity * planes[i] ) >= 0 ) { + if ((clipVelocity * planes[i]) >= 0) { continue; } // slide the original velocity along the crease - dir = planes[i].Cross( planes[j] ); + dir = planes[i].Cross(planes[j]); dir.Normalize(); d = dir * current.velocity; clipVelocity = d * dir; - dir = planes[i].Cross( planes[j] ); + dir = planes[i].Cross(planes[j]); dir.Normalize(); d = dir * endVelocity; endClipVelocity = d * dir; // see if there is a third plane the the new move enters - for ( k = 0; k < numplanes; k++ ) { - if ( k == i || k == j ) { + for (k = 0; k < numplanes; k++) { + if (k == i || k == j) { continue; } - if ( ( clipVelocity * planes[k] ) >= 0.1f ) { + if ((clipVelocity * planes[k]) >= 0.1f) { continue; // move doesn't interact with the plane } @@ -405,29 +484,29 @@ bool idPhysics_Player::SlideMove( bool gravity, bool stepUp, bool stepDown, bool } // step down - if ( stepDown && groundPlane ) { + if (stepDown && groundPlane) { stepEnd = current.origin + gravityNormal * maxStepHeight; - gameLocal.clip.Translation( downTrace, current.origin, stepEnd, clipModel, clipModel->GetAxis(), clipMask, self ); - if ( downTrace.fraction > 1e-4f && downTrace.fraction < 1.0f ) { - current.stepUp -= ( downTrace.endpos - current.origin ) * gravityNormal; + gameLocal.clip.Translation(downTrace, current.origin, stepEnd, clipModel, clipModel->GetAxis(), clipMask, self); + if (downTrace.fraction > 1e-4f && downTrace.fraction < 1.0f) { + current.stepUp -= (downTrace.endpos - current.origin) * gravityNormal; current.origin = downTrace.endpos; current.movementFlags |= PMF_STEPPED_DOWN; current.velocity *= PM_STEPSCALE; } } - if ( gravity ) { + if (gravity) { current.velocity = endVelocity; } // come to a dead stop when the velocity orthogonal to the gravity flipped clipVelocity = current.velocity - gravityNormal * current.velocity * gravityNormal; endClipVelocity = endVelocity - gravityNormal * endVelocity * gravityNormal; - if ( clipVelocity * endClipVelocity < 0.0f ) { + if (clipVelocity * endClipVelocity < 0.0f) { current.velocity = gravityNormal * current.velocity * gravityNormal; } - return (bool)( bumpcount == 0 ); + return (bool)(bumpcount == 0); } /* @@ -437,23 +516,27 @@ idPhysics_Player::Friction Handles both ground friction and water friction ================== */ -void idPhysics_Player::Friction( void ) { +void idPhysics_Player::Friction(void) { idVec3 vel; float speed, newspeed, control; float drop; - + vel = current.velocity; - if ( walking ) { + if (walking) { // ignore slope movement, remove all velocity in gravity direction vel += (vel * gravityNormal) * gravityNormal; } speed = vel.Length(); - if ( speed < 1.0f ) { + if (speed < 1.0f) { // remove all movement orthogonal to gravity, allows for sinking underwater - if ( fabs( current.velocity * gravityNormal ) < 1e-5f ) { + if (fabs(current.velocity * gravityNormal) < 1e-5f) { current.velocity.Zero(); - } else { + } + else if (current.movementType == PM_SPECTATOR) { + current.velocity.Zero(); + } + else { current.velocity = (current.velocity * gravityNormal) * gravityNormal; } // FIXME: still have z friction underwater? @@ -463,22 +546,22 @@ void idPhysics_Player::Friction( void ) { drop = 0; // spectator friction - if ( current.movementType == PM_SPECTATOR ) { + if (current.movementType == PM_SPECTATOR) { drop += speed * PM_FLYFRICTION * frametime; } // apply ground friction - else if ( walking && waterLevel <= WATERLEVEL_FEET ) { + else if (walking && waterLevel <= WATERLEVEL_FEET) { // no friction on slick surfaces - if ( !(groundMaterial && groundMaterial->GetSurfaceFlags() & SURF_SLICK) ) { + if (!(groundMaterial && groundMaterial->GetSurfaceFlags() & SURF_SLICK)) { // if getting knocked back, no friction - if ( !(current.movementFlags & PMF_TIME_KNOCKBACK) ) { + if (!(current.movementFlags & PMF_TIME_KNOCKBACK)) { control = speed < PM_STOPSPEED ? PM_STOPSPEED : speed; drop += control * PM_FRICTION * frametime; } } } // apply water friction even if just wading - else if ( waterLevel ) { + else if (waterLevel) { drop += speed * PM_WATERFRICTION * waterLevel * frametime; } // apply air friction @@ -491,7 +574,10 @@ void idPhysics_Player::Friction( void ) { if (newspeed < 0) { newspeed = 0; } - current.velocity *= ( newspeed / speed ); + current.velocity *= (newspeed / speed); + + // snap tiny components away to avoid denormal slow paths in long-running physics frames + current.velocity.FixDenormals(); } /* @@ -501,15 +587,15 @@ idPhysics_Player::WaterJumpMove Flying out of the water =================== */ -void idPhysics_Player::WaterJumpMove( void ) { +void idPhysics_Player::WaterJumpMove(void) { // waterjump has no control, but falls - idPhysics_Player::SlideMove( true, true, false, false ); + idPhysics_Player::SlideMove(true, true, false, false); // add gravity current.velocity += gravityNormal * frametime; // if falling down - if ( current.velocity * gravityNormal > 0.0f ) { + if (current.velocity * gravityNormal > 0.0f) { // cancel as soon as we are falling down again current.movementFlags &= ~PMF_ALL_TIMES; current.movementTime = 0; @@ -521,26 +607,27 @@ void idPhysics_Player::WaterJumpMove( void ) { idPhysics_Player::WaterMove =================== */ -void idPhysics_Player::WaterMove( void ) { +void idPhysics_Player::WaterMove(void) { idVec3 wishvel; float wishspeed; idVec3 wishdir; float scale; float vel; - if ( idPhysics_Player::CheckWaterJump() ) { + if (idPhysics_Player::CheckWaterJump()) { idPhysics_Player::WaterJumpMove(); return; } idPhysics_Player::Friction(); - scale = idPhysics_Player::CmdScale( command ); + scale = idPhysics_Player::CmdScale(command); // user intentions - if ( !scale ) { + if (!scale) { wishvel = gravityNormal * 60; // sink towards bottom - } else { + } + else { wishvel = scale * (viewForward * command.forwardmove + viewRight * command.rightmove); wishvel -= scale * gravityNormal * command.upmove; } @@ -548,23 +635,23 @@ void idPhysics_Player::WaterMove( void ) { wishdir = wishvel; wishspeed = wishdir.Normalize(); - if ( wishspeed > playerSpeed * PM_SWIMSCALE ) { + if (wishspeed > playerSpeed * PM_SWIMSCALE) { wishspeed = playerSpeed * PM_SWIMSCALE; } - idPhysics_Player::Accelerate( wishdir, wishspeed, PM_WATERACCELERATE ); + idPhysics_Player::Accelerate(wishdir, wishspeed, PM_WATERACCELERATE); // make sure we can go up slopes easily under water - if ( groundPlane && ( current.velocity * groundTrace.c.normal ) < 0.0f ) { + if (groundPlane && (current.velocity * groundTrace.c.normal) < 0.0f) { vel = current.velocity.Length(); // slide along the ground plane - current.velocity.ProjectOntoPlane( groundTrace.c.normal, OVERCLIP ); + current.velocity.ProjectOntoPlane(groundTrace.c.normal, OVERCLIP); current.velocity.Normalize(); current.velocity *= vel; } - idPhysics_Player::SlideMove( false, true, false, false ); + idPhysics_Player::SlideMove(false, true, false, false); } /* @@ -572,7 +659,7 @@ void idPhysics_Player::WaterMove( void ) { idPhysics_Player::FlyMove =================== */ -void idPhysics_Player::FlyMove( void ) { +void idPhysics_Player::FlyMove(void) { idVec3 wishvel; float wishspeed; idVec3 wishdir; @@ -581,11 +668,12 @@ void idPhysics_Player::FlyMove( void ) { // normal slowdown idPhysics_Player::Friction(); - scale = idPhysics_Player::CmdScale( command ); + scale = idPhysics_Player::CmdScale(command); - if ( !scale ) { + if (!scale) { wishvel = vec3_origin; - } else { + } + else { wishvel = scale * (viewForward * command.forwardmove + viewRight * command.rightmove); wishvel -= scale * gravityNormal * command.upmove; } @@ -593,9 +681,9 @@ void idPhysics_Player::FlyMove( void ) { wishdir = wishvel; wishspeed = wishdir.Normalize(); - idPhysics_Player::Accelerate( wishdir, wishspeed, PM_FLYACCELERATE ); + idPhysics_Player::Accelerate(wishdir, wishspeed, PM_FLYACCELERATE); - idPhysics_Player::SlideMove( false, false, false, false ); + idPhysics_Player::SlideMove(false, false, false, false); } /* @@ -603,7 +691,7 @@ void idPhysics_Player::FlyMove( void ) { idPhysics_Player::AirMove =================== */ -void idPhysics_Player::AirMove( void ) { +void idPhysics_Player::AirMove(void) { idVec3 wishvel; idVec3 wishdir; float wishspeed; @@ -611,7 +699,7 @@ void idPhysics_Player::AirMove( void ) { idPhysics_Player::Friction(); - scale = idPhysics_Player::CmdScale( command ); + scale = PM_CmdScale(command, playerSpeed, true); // project moves down to flat plane viewForward -= (viewForward * gravityNormal) * gravityNormal; @@ -626,16 +714,16 @@ void idPhysics_Player::AirMove( void ) { wishspeed *= scale; // not on ground, so little effect on velocity - idPhysics_Player::Accelerate( wishdir, wishspeed, PM_AIRACCELERATE ); + idPhysics_Player::Accelerate(wishdir, wishspeed, PM_AIRACCELERATE); // we may have a ground plane that is very steep, even // though we don't have a groundentity // slide along the steep plane - if ( groundPlane ) { - current.velocity.ProjectOntoPlane( groundTrace.c.normal, OVERCLIP ); + if (groundPlane) { + current.velocity.ProjectOntoPlane(groundTrace.c.normal, OVERCLIP); } - idPhysics_Player::SlideMove( true, false, false, false ); + idPhysics_Player::SlideMove(true, false, false, false); } /* @@ -643,24 +731,23 @@ void idPhysics_Player::AirMove( void ) { idPhysics_Player::WalkMove =================== */ -void idPhysics_Player::WalkMove( void ) { +void idPhysics_Player::WalkMove(void) { idVec3 wishvel; idVec3 wishdir; float wishspeed; float scale; float accelerate; - idVec3 oldVelocity, vel; - float oldVel, newVel; + idVec3 vel; - if ( waterLevel > WATERLEVEL_WAIST && ( viewForward * groundTrace.c.normal ) > 0.0f ) { + if (waterLevel > WATERLEVEL_WAIST && (viewForward * groundTrace.c.normal) > 0.0f) { // begin swimming idPhysics_Player::WaterMove(); return; } - if ( idPhysics_Player::CheckJump() ) { + if (idPhysics_Player::CheckJump()) { // jumped away - if ( waterLevel > WATERLEVEL_FEET ) { + if (waterLevel > WATERLEVEL_FEET) { idPhysics_Player::WaterMove(); } else { @@ -671,16 +758,16 @@ void idPhysics_Player::WalkMove( void ) { idPhysics_Player::Friction(); - scale = idPhysics_Player::CmdScale( command ); + scale = PM_CmdScale(command, playerSpeed, true); // project moves down to flat plane viewForward -= (viewForward * gravityNormal) * gravityNormal; viewRight -= (viewRight * gravityNormal) * gravityNormal; - // project the forward and right directions onto the ground plane - viewForward.ProjectOntoPlane( groundTrace.c.normal, OVERCLIP ); - viewRight.ProjectOntoPlane( groundTrace.c.normal, OVERCLIP ); - // + // project the forward and right directions onto the ground plane while preserving + // their horizontal components, matching ETQW's slope movement fix. + viewForward = PM_AdjustVertically(groundTrace.c.normal, viewForward, gravityNormal); + viewRight = PM_AdjustVertically(groundTrace.c.normal, viewRight, gravityNormal); viewForward.Normalize(); viewRight.Normalize(); @@ -690,56 +777,42 @@ void idPhysics_Player::WalkMove( void ) { wishspeed *= scale; // clamp the speed lower if wading or walking on the bottom - if ( waterLevel ) { + if (waterLevel) { float waterScale; waterScale = waterLevel / 3.0f; - waterScale = 1.0f - ( 1.0f - PM_SWIMSCALE ) * waterScale; - if ( wishspeed > playerSpeed * waterScale ) { + waterScale = 1.0f - (1.0f - PM_SWIMSCALE) * waterScale; + if (wishspeed > playerSpeed * waterScale) { wishspeed = playerSpeed * waterScale; } } // when a player gets hit, they temporarily lose full control, which allows them to be moved a bit - if ( ( groundMaterial && groundMaterial->GetSurfaceFlags() & SURF_SLICK ) || current.movementFlags & PMF_TIME_KNOCKBACK ) { + if ((groundMaterial && groundMaterial->GetSurfaceFlags() & SURF_SLICK) || current.movementFlags & PMF_TIME_KNOCKBACK) { accelerate = PM_AIRACCELERATE; } else { accelerate = PM_ACCELERATE; } - idPhysics_Player::Accelerate( wishdir, wishspeed, accelerate ); + idPhysics_Player::Accelerate(wishdir, wishspeed, accelerate); - if ( ( groundMaterial && groundMaterial->GetSurfaceFlags() & SURF_SLICK ) || current.movementFlags & PMF_TIME_KNOCKBACK ) { + if ((groundMaterial && groundMaterial->GetSurfaceFlags() & SURF_SLICK) || current.movementFlags & PMF_TIME_KNOCKBACK) { current.velocity += gravityVector * frametime; } - oldVelocity = current.velocity; - - // slide along the ground plane - current.velocity.ProjectOntoPlane( groundTrace.c.normal, OVERCLIP ); - - // if not clipped into the opposite direction - if ( oldVelocity * current.velocity > 0.0f ) { - newVel = current.velocity.LengthSqr(); - if ( newVel > 1.0f ) { - oldVel = oldVelocity.LengthSqr(); - if ( oldVel > 1.0f ) { - // don't decrease velocity when going up or down a slope - current.velocity *= idMath::Sqrt( oldVel / newVel ); - } - } - } + // slide along the ground plane without losing horizontal speed on walkable slopes + current.velocity = PM_AdjustVertically(groundTrace.c.normal, current.velocity, gravityNormal); // don't do anything if standing still vel = current.velocity - (current.velocity * gravityNormal) * gravityNormal; - if ( !vel.LengthSqr() ) { + if (!vel.LengthSqr()) { return; } gameLocal.push.InitSavingPushedEntityPositions(); - idPhysics_Player::SlideMove( false, true, true, true ); + idPhysics_Player::SlideMove(false, true, true, true); } /* @@ -747,22 +820,14 @@ void idPhysics_Player::WalkMove( void ) { idPhysics_Player::DeadMove ============== */ -void idPhysics_Player::DeadMove( void ) { - float forward; - - if ( !walking ) { - return; - } - - // extra friction - forward = current.velocity.Length(); - forward -= 20; - if ( forward <= 0 ) { - current.velocity = vec3_origin; +void idPhysics_Player::DeadMove(void) { + // ETQW keeps dead players moving through the same ground/air paths with input + // already zeroed by MovePlayer, so gravity and sliding still resolve correctly. + if (walking) { + idPhysics_Player::WalkMove(); } else { - current.velocity.Normalize(); - current.velocity *= forward; + idPhysics_Player::AirMove(); } } @@ -771,19 +836,19 @@ void idPhysics_Player::DeadMove( void ) { idPhysics_Player::NoclipMove =============== */ -void idPhysics_Player::NoclipMove( void ) { +void idPhysics_Player::NoclipMove(void) { float speed, drop, friction, newspeed, stopspeed; float scale, wishspeed; idVec3 wishdir; // friction speed = current.velocity.Length(); - if ( speed < 20.0f ) { + if (speed < 20.0f) { current.velocity = vec3_origin; } else { stopspeed = playerSpeed * 0.3f; - if ( speed < stopspeed ) { + if (speed < stopspeed) { speed = stopspeed; } friction = PM_NOCLIPFRICTION; @@ -799,14 +864,14 @@ void idPhysics_Player::NoclipMove( void ) { } // accelerate - scale = idPhysics_Player::CmdScale( command ); + scale = idPhysics_Player::CmdScale(command); wishdir = scale * (viewForward * command.forwardmove + viewRight * command.rightmove); wishdir -= scale * gravityNormal * command.upmove; wishspeed = wishdir.Normalize(); wishspeed *= scale; - idPhysics_Player::Accelerate( wishdir, wishspeed, PM_ACCELERATE ); + idPhysics_Player::Accelerate(wishdir, wishspeed, PM_ACCELERATE); // move current.origin += frametime * current.velocity; @@ -817,7 +882,7 @@ void idPhysics_Player::NoclipMove( void ) { idPhysics_Player::SpectatorMove =============== */ -void idPhysics_Player::SpectatorMove( void ) { +void idPhysics_Player::SpectatorMove(void) { idVec3 wishvel; float wishspeed; idVec3 wishdir; @@ -830,20 +895,22 @@ void idPhysics_Player::SpectatorMove( void ) { idPhysics_Player::Friction(); - scale = idPhysics_Player::CmdScale( command ); + scale = idPhysics_Player::CmdScale(command); - if ( !scale ) { + if (!scale) { wishvel = vec3_origin; - } else { + } + else { wishvel = scale * (viewForward * command.forwardmove + viewRight * command.rightmove); + wishvel -= scale * gravityNormal * command.upmove; } wishdir = wishvel; wishspeed = wishdir.Normalize(); - idPhysics_Player::Accelerate( wishdir, wishspeed, PM_FLYACCELERATE ); + idPhysics_Player::Accelerate(wishdir, wishspeed, PM_FLYACCELERATE); - idPhysics_Player::SlideMove( false, false, false, false ); + idPhysics_Player::SlideMove(false, false, false, false); } /* @@ -851,7 +918,7 @@ void idPhysics_Player::SpectatorMove( void ) { idPhysics_Player::LadderMove ============ */ -void idPhysics_Player::LadderMove( void ) { +void idPhysics_Player::LadderMove(void) { idVec3 wishdir, wishvel, right; float wishspeed, scale; float upscale; @@ -861,18 +928,18 @@ void idPhysics_Player::LadderMove( void ) { current.velocity = (gravityNormal * current.velocity) * gravityNormal + wishvel; upscale = (-gravityNormal * viewForward + 0.5f) * 2.5f; - if ( upscale > 1.0f ) { + if (upscale > 1.0f) { upscale = 1.0f; } - else if ( upscale < -1.0f ) { + else if (upscale < -1.0f) { upscale = -1.0f; } - scale = idPhysics_Player::CmdScale( command ); + scale = idPhysics_Player::CmdScale(command); wishvel = -0.9f * gravityNormal * upscale * scale * (float)command.forwardmove; // strafe - if ( command.rightmove ) { + if (command.rightmove) { // right vector orthogonal to gravity right = viewRight - (gravityNormal * viewRight) * gravityNormal; // project right vector into ladder plane @@ -880,15 +947,15 @@ void idPhysics_Player::LadderMove( void ) { right.Normalize(); // if we are looking away from the ladder, reverse the right vector - if ( ladderNormal * viewForward > 0.0f ) { + if (ladderNormal * viewForward > 0.0f) { right = -right; } - wishvel += 2.0f * right * scale * (float) command.rightmove; + wishvel += 2.0f * right * scale * (float)command.rightmove; } // up down movement - if ( command.upmove ) { - wishvel += -0.5f * gravityNormal * scale * (float) command.upmove; + if (command.upmove) { + wishvel += -0.5f * gravityNormal * scale * (float)command.upmove; } // do strafe friction @@ -896,33 +963,33 @@ void idPhysics_Player::LadderMove( void ) { // accelerate wishspeed = wishvel.Normalize(); - idPhysics_Player::Accelerate( wishvel, wishspeed, PM_ACCELERATE ); + idPhysics_Player::Accelerate(wishvel, wishspeed, PM_ACCELERATE); // cap the vertical velocity upscale = current.velocity * -gravityNormal; - if ( upscale < -PM_LADDERSPEED ) { + if (upscale < -PM_LADDERSPEED) { current.velocity += gravityNormal * (upscale + PM_LADDERSPEED); } - else if ( upscale > PM_LADDERSPEED ) { + else if (upscale > PM_LADDERSPEED) { current.velocity += gravityNormal * (upscale - PM_LADDERSPEED); } - if ( (wishvel * gravityNormal) == 0.0f ) { - if ( current.velocity * gravityNormal < 0.0f ) { + if ((wishvel * gravityNormal) == 0.0f) { + if (current.velocity * gravityNormal < 0.0f) { current.velocity += gravityVector * frametime; - if ( current.velocity * gravityNormal > 0.0f ) { + if (current.velocity * gravityNormal > 0.0f) { current.velocity -= (gravityNormal * current.velocity) * gravityNormal; } } else { current.velocity -= gravityVector * frametime; - if ( current.velocity * gravityNormal < 0.0f ) { + if (current.velocity * gravityNormal < 0.0f) { current.velocity -= (gravityNormal * current.velocity) * gravityNormal; } } } - idPhysics_Player::SlideMove( false, ( command.forwardmove > 0 ), false, false ); + idPhysics_Player::SlideMove(false, (command.forwardmove > 0), false, false); } /* @@ -930,20 +997,20 @@ void idPhysics_Player::LadderMove( void ) { idPhysics_Player::CorrectAllSolid ============= */ -void idPhysics_Player::CorrectAllSolid( trace_t &trace, int contents ) { - if ( debugLevel ) { - gameLocal.Printf( "%i:allsolid\n", c_pmove ); +void idPhysics_Player::CorrectAllSolid(trace_t& trace, int contents) { + if (debugLevel) { + gameLocal.Printf("%i:allsolid\n", c_pmove); } // FIXME: jitter around to find a free spot ? - if ( trace.fraction >= 1.0f ) { - memset( &trace, 0, sizeof( trace ) ); + if (trace.fraction >= 1.0f) { + memset(&trace, 0, sizeof(trace)); trace.endpos = current.origin; trace.endAxis = clipModelAxis; trace.fraction = 0.0f; trace.c.dist = current.origin.z; - trace.c.normal.Set( 0, 0, 1 ); + trace.c.normal.Set(0, 0, 1); trace.c.point = current.origin; trace.c.entityNum = ENTITYNUM_WORLD; trace.c.id = 0; @@ -958,7 +1025,7 @@ void idPhysics_Player::CorrectAllSolid( trace_t &trace, int contents ) { idPhysics_Player::CheckGround ============= */ -void idPhysics_Player::CheckGround( void ) { +void idPhysics_Player::CheckGround(void) { int i, contents; idVec3 point; bool hadGroundContacts; @@ -966,32 +1033,33 @@ void idPhysics_Player::CheckGround( void ) { hadGroundContacts = HasGroundContacts(); // set the clip model origin before getting the contacts - clipModel->SetPosition( current.origin, clipModel->GetAxis() ); + clipModel->SetPosition(current.origin, clipModel->GetAxis()); EvaluateContacts(); // setup a ground trace from the contacts groundTrace.endpos = current.origin; groundTrace.endAxis = clipModel->GetAxis(); - if ( contacts.Num() ) { + if (contacts.Num()) { groundTrace.fraction = 0.0f; groundTrace.c = contacts[0]; - for ( i = 1; i < contacts.Num(); i++ ) { + for (i = 1; i < contacts.Num(); i++) { groundTrace.c.normal += contacts[i].normal; } groundTrace.c.normal.Normalize(); - } else { + } + else { groundTrace.fraction = 1.0f; } - contents = gameLocal.clip.Contents( current.origin, clipModel, clipModel->GetAxis(), -1, self ); - if ( contents & MASK_SOLID ) { + contents = gameLocal.clip.Contents(current.origin, clipModel, clipModel->GetAxis(), -1, self); + if (contents & MASK_SOLID) { // do something corrective if stuck in solid - idPhysics_Player::CorrectAllSolid( groundTrace, contents ); + idPhysics_Player::CorrectAllSolid(groundTrace, contents); } // if the trace didn't hit anything, we are in free fall - if ( groundTrace.fraction == 1.0f ) { + if (groundTrace.fraction == 1.0f) { groundPlane = false; walking = false; groundEntityPtr = NULL; @@ -999,30 +1067,30 @@ void idPhysics_Player::CheckGround( void ) { } groundMaterial = groundTrace.c.material; - groundEntityPtr = gameLocal.entities[ groundTrace.c.entityNum ]; + groundEntityPtr = gameLocal.entities[groundTrace.c.entityNum]; // check if getting thrown off the ground - if ( (current.velocity * -gravityNormal) > 0.0f && ( current.velocity * groundTrace.c.normal ) > 10.0f ) { - if ( debugLevel ) { - gameLocal.Printf( "%i:kickoff\n", c_pmove ); + if ((current.velocity * -gravityNormal) > 0.0f && (current.velocity * groundTrace.c.normal) > 1.0f) { + if (debugLevel) { + gameLocal.Printf("%i:kickoff\n", c_pmove); } groundPlane = false; walking = false; return; } - + // slopes that are too steep will not be considered onground - if ( ( groundTrace.c.normal * -gravityNormal ) < MIN_WALK_NORMAL ) { - if ( debugLevel ) { - gameLocal.Printf( "%i:steep\n", c_pmove ); + if ((groundTrace.c.normal * -gravityNormal) < MIN_WALK_NORMAL) { + if (debugLevel) { + gameLocal.Printf("%i:steep\n", c_pmove); } // FIXME: if they can't slide down the slope, let them walk (sharp crevices) // make sure we don't die from sliding down a steep slope - if ( current.velocity * gravityNormal > 150.0f ) { - current.velocity -= ( current.velocity * gravityNormal - 150.0f ) * gravityNormal; + if (current.velocity * gravityNormal > 150.0f) { + current.velocity -= (current.velocity * gravityNormal - 150.0f) * gravityNormal; } groundPlane = true; @@ -1034,16 +1102,16 @@ void idPhysics_Player::CheckGround( void ) { walking = true; // hitting solid ground will end a waterjump - if ( current.movementFlags & PMF_TIME_WATERJUMP ) { - current.movementFlags &= ~( PMF_TIME_WATERJUMP | PMF_TIME_LAND ); + if (current.movementFlags & PMF_TIME_WATERJUMP) { + current.movementFlags &= ~(PMF_TIME_WATERJUMP | PMF_TIME_LAND); current.movementTime = 0; } // if the player didn't have ground contacts the previous frame - if ( !hadGroundContacts ) { + if (!hadGroundContacts) { // don't do landing time if we were just going down a slope - if ( (current.velocity * -gravityNormal) < -200.0f ) { + if ((current.velocity * -gravityNormal) < -200.0f) { // don't allow another jump for a little while current.movementFlags |= PMF_TIME_LAND; current.movementTime = 250; @@ -1051,13 +1119,13 @@ void idPhysics_Player::CheckGround( void ) { } // let the entity know about the collision - self->Collide( groundTrace, current.velocity ); + self->Collide(groundTrace, current.velocity); - if ( groundEntityPtr.GetEntity() ) { + if (groundEntityPtr.GetEntity()) { impactInfo_t info; - groundEntityPtr.GetEntity()->GetImpactInfo( self, groundTrace.c.id, groundTrace.c.point, &info ); - if ( info.invMass != 0.0f ) { - groundEntityPtr.GetEntity()->ApplyImpulse( self, groundTrace.c.id, groundTrace.c.point, current.velocity / ( info.invMass * 10.0f ) ); + groundEntityPtr.GetEntity()->GetImpactInfo(self, groundTrace.c.id, groundTrace.c.point, &info); + if (info.invMass != 0.0f) { + groundEntityPtr.GetEntity()->ApplyImpulse(self, groundTrace.c.id, groundTrace.c.point, current.velocity / (info.invMass * 10.0f)); } } } @@ -1069,47 +1137,51 @@ idPhysics_Player::CheckDuck Sets clip model size ============== */ -void idPhysics_Player::CheckDuck( void ) { +void idPhysics_Player::CheckDuck(void) { trace_t trace; idVec3 end; idBounds bounds; float maxZ; - if ( current.movementType == PM_DEAD ) { + if (current.movementType == PM_DEAD) { maxZ = pm_deadheight.GetFloat(); - } else { - // stand up when up against a ladder - if ( command.upmove < 0 && !ladder ) { + } + else { + // stand up when up against a ladder, and do not duck while swimming deeply + if (command.upmove < 0 && !ladder && waterLevel <= WATERLEVEL_FEET) { // duck current.movementFlags |= PMF_DUCKED; - } else { + } + else { // stand up if possible - if ( current.movementFlags & PMF_DUCKED ) { + if (current.movementFlags & PMF_DUCKED) { // try to stand up - end = current.origin - ( pm_normalheight.GetFloat() - pm_crouchheight.GetFloat() ) * gravityNormal; - gameLocal.clip.Translation( trace, current.origin, end, clipModel, clipModel->GetAxis(), clipMask, self ); - if ( trace.fraction >= 1.0f ) { + end = current.origin - (pm_normalheight.GetFloat() - pm_crouchheight.GetFloat()) * gravityNormal; + gameLocal.clip.Translation(trace, current.origin, end, clipModel, clipModel->GetAxis(), clipMask, self); + if (trace.fraction >= 1.0f) { current.movementFlags &= ~PMF_DUCKED; } } } - if ( current.movementFlags & PMF_DUCKED ) { + if (current.movementFlags & PMF_DUCKED) { playerSpeed = crouchSpeed; maxZ = pm_crouchheight.GetFloat(); - } else { + } + else { maxZ = pm_normalheight.GetFloat(); } } // if the clipModel height should change - if ( clipModel->GetBounds()[1][2] != maxZ ) { + if (clipModel->GetBounds()[1][2] != maxZ) { bounds = clipModel->GetBounds(); bounds[1][2] = maxZ; - if ( pm_usecylinder.GetBool() ) { - clipModel->LoadModel( idTraceModel( bounds, 8 ) ); - } else { - clipModel->LoadModel( idTraceModel( bounds ) ); + if (pm_usecylinder.GetBool()) { + clipModel->LoadModel(idTraceModel(bounds, 8)); + } + else { + clipModel->LoadModel(idTraceModel(bounds)); } } } @@ -1119,17 +1191,17 @@ void idPhysics_Player::CheckDuck( void ) { idPhysics_Player::CheckLadder ================ */ -void idPhysics_Player::CheckLadder( void ) { +void idPhysics_Player::CheckLadder(void) { idVec3 forward, start, end; trace_t trace; float tracedist; - - if ( current.movementTime ) { + + if (current.movementTime) { return; } // if on the ground moving backwards - if ( walking && command.forwardmove <= 0 ) { + if (walking && command.forwardmove <= 0) { return; } @@ -1137,34 +1209,35 @@ void idPhysics_Player::CheckLadder( void ) { forward = viewForward - (gravityNormal * viewForward) * gravityNormal; forward.Normalize(); - if ( walking ) { + if (walking) { // don't want to get sucked towards the ladder when still walking tracedist = 1.0f; - } else { + } + else { tracedist = 48.0f; } end = current.origin + tracedist * forward; - gameLocal.clip.Translation( trace, current.origin, end, clipModel, clipModel->GetAxis(), clipMask, self ); + gameLocal.clip.Translation(trace, current.origin, end, clipModel, clipModel->GetAxis(), clipMask, self); // if near a surface - if ( trace.fraction < 1.0f ) { + if (trace.fraction < 1.0f) { // if a ladder surface - if ( trace.c.material && ( trace.c.material->GetSurfaceFlags() & SURF_LADDER ) ) { + if (trace.c.material && (trace.c.material->GetSurfaceFlags() & SURF_LADDER)) { // check a step height higher - end = current.origin - gravityNormal * ( maxStepHeight * 0.75f ); - gameLocal.clip.Translation( trace, current.origin, end, clipModel, clipModel->GetAxis(), clipMask, self ); + end = current.origin - gravityNormal * (maxStepHeight * 0.75f); + gameLocal.clip.Translation(trace, current.origin, end, clipModel, clipModel->GetAxis(), clipMask, self); start = trace.endpos; end = start + tracedist * forward; - gameLocal.clip.Translation( trace, start, end, clipModel, clipModel->GetAxis(), clipMask, self ); + gameLocal.clip.Translation(trace, start, end, clipModel, clipModel->GetAxis(), clipMask, self); // if also near a surface a step height higher - if ( trace.fraction < 1.0f ) { + if (trace.fraction < 1.0f) { // if it also is a ladder surface - if ( trace.c.material && trace.c.material->GetSurfaceFlags() & SURF_LADDER ) { + if (trace.c.material && trace.c.material->GetSurfaceFlags() & SURF_LADDER) { ladder = true; ladderNormal = trace.c.normal; } @@ -1178,30 +1251,40 @@ void idPhysics_Player::CheckLadder( void ) { idPhysics_Player::CheckJump ============= */ -bool idPhysics_Player::CheckJump( void ) { +bool idPhysics_Player::CheckJump(void) { idVec3 addVelocity; - if ( command.upmove < 10 ) { + if (command.upmove < 10) { // not holding jump return false; } // must wait for jump to be released - if ( current.movementFlags & PMF_JUMP_HELD ) { + if (current.movementFlags & PMF_JUMP_HELD) { + return false; + } + + if (current.movementFlags & PMF_TIME_LAND) { return false; } // don't jump if we can't stand up - if ( current.movementFlags & PMF_DUCKED ) { + if (current.movementFlags & PMF_DUCKED) { return false; } + // Make sure the current velocity is leaving the walkable ground plane before + // adding jump velocity; this avoids sticking or losing jump speed on slopes. + if (groundPlane && PM_IsWalkablePlane(groundTrace.c.normal, gravityNormal) && (current.velocity * groundTrace.c.normal) < 0.0f) { + current.velocity = PM_AdjustVertically(groundTrace.c.normal, current.velocity, gravityNormal); + } + groundPlane = false; // jumping away walking = false; current.movementFlags |= PMF_JUMP_HELD | PMF_JUMPED; addVelocity = 2.0f * maxJumpHeight * -gravityVector; - addVelocity *= idMath::Sqrt( addVelocity.Normalize() ); + addVelocity *= idMath::Sqrt(addVelocity.Normalize()); current.velocity += addVelocity; return true; @@ -1212,17 +1295,21 @@ bool idPhysics_Player::CheckJump( void ) { idPhysics_Player::CheckWaterJump ============= */ -bool idPhysics_Player::CheckWaterJump( void ) { +bool idPhysics_Player::CheckWaterJump(void) { idVec3 spot; int cont; idVec3 flatforward; - if ( current.movementTime ) { + if (current.movementTime) { + return false; + } + + if (command.forwardmove <= 0) { return false; } // check for water jump - if ( waterLevel != WATERLEVEL_WAIST ) { + if (waterLevel != WATERLEVEL_WAIST) { return false; } @@ -1231,19 +1318,21 @@ bool idPhysics_Player::CheckWaterJump( void ) { spot = current.origin + 30.0f * flatforward; spot -= 4.0f * gravityNormal; - cont = gameLocal.clip.Contents( spot, NULL, mat3_identity, -1, self ); - if ( !(cont & CONTENTS_SOLID) ) { + cont = gameLocal.clip.Contents(spot, NULL, mat3_identity, CONTENTS_SOLID, self); + if (!(cont & CONTENTS_SOLID)) { return false; } - spot -= 16.0f * gravityNormal; - cont = gameLocal.clip.Contents( spot, NULL, mat3_identity, -1, self ); - if ( cont ) { + float height = clipModel->GetBounds().Size().z; + + spot -= (height * 0.75f) * gravityNormal; + cont = gameLocal.clip.Contents(spot, NULL, mat3_identity, -1, self); + if (cont) { return false; } // jump out of water - current.velocity = 200.0f * viewForward - 350.0f * gravityNormal; + current.velocity = 200.0f * viewForward - 400.0f * gravityNormal; current.movementFlags |= PMF_TIME_WATERJUMP; current.movementTime = 2000; @@ -1255,7 +1344,7 @@ bool idPhysics_Player::CheckWaterJump( void ) { idPhysics_Player::SetWaterLevel ============= */ -void idPhysics_Player::SetWaterLevel( void ) { +void idPhysics_Player::SetWaterLevel(void) { idVec3 point; idBounds bounds; int contents; @@ -1269,24 +1358,24 @@ void idPhysics_Player::SetWaterLevel( void ) { bounds = clipModel->GetBounds(); // check at feet level - point = current.origin - ( bounds[0][2] + 1.0f ) * gravityNormal; - contents = gameLocal.clip.Contents( point, NULL, mat3_identity, -1, self ); - if ( contents & MASK_WATER ) { + point = current.origin - (bounds[0][2] + 1.0f) * gravityNormal; + contents = gameLocal.clip.Contents(point, NULL, mat3_identity, -1, self); + if (contents & MASK_WATER) { waterType = contents; waterLevel = WATERLEVEL_FEET; // check at waist level - point = current.origin - ( bounds[1][2] - bounds[0][2] ) * 0.5f * gravityNormal; - contents = gameLocal.clip.Contents( point, NULL, mat3_identity, -1, self ); - if ( contents & MASK_WATER ) { + point = current.origin - (bounds[1][2] - bounds[0][2]) * 0.5f * gravityNormal; + contents = gameLocal.clip.Contents(point, NULL, mat3_identity, -1, self); + if (contents & MASK_WATER) { waterLevel = WATERLEVEL_WAIST; // check at head level - point = current.origin - ( bounds[1][2] - 1.0f ) * gravityNormal; - contents = gameLocal.clip.Contents( point, NULL, mat3_identity, -1, self ); - if ( contents & MASK_WATER ) { + point = current.origin - (bounds[1][2] - 1.0f) * gravityNormal; + contents = gameLocal.clip.Contents(point, NULL, mat3_identity, -1, self); + if (contents & MASK_WATER) { waterLevel = WATERLEVEL_HEAD; } } @@ -1298,10 +1387,10 @@ void idPhysics_Player::SetWaterLevel( void ) { idPhysics_Player::DropTimers ================ */ -void idPhysics_Player::DropTimers( void ) { +void idPhysics_Player::DropTimers(void) { // drop misc timing counter - if ( current.movementTime ) { - if ( framemsec >= current.movementTime ) { + if (current.movementTime) { + if (framemsec >= current.movementTime) { current.movementFlags &= ~PMF_ALL_TIMES; current.movementTime = 0; } @@ -1316,7 +1405,7 @@ void idPhysics_Player::DropTimers( void ) { idPhysics_Player::MovePlayer ================ */ -void idPhysics_Player::MovePlayer( int msec ) { +void idPhysics_Player::MovePlayer(int msec) { // this counter lets us debug movement problems with a journal // by setting a conditional breakpoint for the previous frame @@ -1334,16 +1423,16 @@ void idPhysics_Player::MovePlayer( int msec ) { playerSpeed = walkSpeed; // remove jumped and stepped up flag - current.movementFlags &= ~(PMF_JUMPED|PMF_STEPPED_UP|PMF_STEPPED_DOWN); + current.movementFlags &= ~(PMF_JUMPED | PMF_STEPPED_UP | PMF_STEPPED_DOWN); current.stepUp = 0.0f; - if ( command.upmove < 10 ) { + if (command.upmove < 10) { // not holding jump current.movementFlags &= ~PMF_JUMP_HELD; } // if no movement at all - if ( current.movementType == PM_FREEZE ) { + if (current.movementType == PM_FREEZE) { return; } @@ -1351,27 +1440,27 @@ void idPhysics_Player::MovePlayer( int msec ) { current.velocity -= current.pushVelocity; // view vectors - viewAngles.ToVectors( &viewForward, NULL, NULL ); + viewAngles.ToVectors(&viewForward, NULL, NULL); viewForward *= clipModelAxis; - viewRight = gravityNormal.Cross( viewForward ); + viewRight = gravityNormal.Cross(viewForward); viewRight.Normalize(); // fly in spectator mode - if ( current.movementType == PM_SPECTATOR ) { + if (current.movementType == PM_SPECTATOR) { SpectatorMove(); idPhysics_Player::DropTimers(); return; } // special no clip mode - if ( current.movementType == PM_NOCLIP ) { + if (current.movementType == PM_NOCLIP) { idPhysics_Player::NoclipMove(); idPhysics_Player::DropTimers(); return; } // no control when dead - if ( current.movementType == PM_DEAD ) { + if (current.movementType == PM_DEAD) { command.forwardmove = 0; command.rightmove = 0; command.upmove = 0; @@ -1393,23 +1482,23 @@ void idPhysics_Player::MovePlayer( int msec ) { idPhysics_Player::DropTimers(); // move - if ( current.movementType == PM_DEAD ) { + if (current.movementType == PM_DEAD) { // dead idPhysics_Player::DeadMove(); } - else if ( ladder ) { + else if (ladder) { // going up or down a ladder idPhysics_Player::LadderMove(); } - else if ( current.movementFlags & PMF_TIME_WATERJUMP ) { + else if (current.movementFlags & PMF_TIME_WATERJUMP) { // jumping out of water idPhysics_Player::WaterJumpMove(); } - else if ( waterLevel > 1 ) { + else if (waterLevel > 1) { // swimming idPhysics_Player::WaterMove(); } - else if ( walking ) { + else if (walking) { // walking on ground idPhysics_Player::WalkMove(); } @@ -1432,7 +1521,7 @@ void idPhysics_Player::MovePlayer( int msec ) { idPhysics_Player::GetWaterLevel ================ */ -waterLevel_t idPhysics_Player::GetWaterLevel( void ) const { +waterLevel_t idPhysics_Player::GetWaterLevel(void) const { return waterLevel; } @@ -1441,7 +1530,7 @@ waterLevel_t idPhysics_Player::GetWaterLevel( void ) const { idPhysics_Player::GetWaterType ================ */ -int idPhysics_Player::GetWaterType( void ) const { +int idPhysics_Player::GetWaterType(void) const { return waterType; } @@ -1450,8 +1539,8 @@ int idPhysics_Player::GetWaterType( void ) const { idPhysics_Player::HasJumped ================ */ -bool idPhysics_Player::HasJumped( void ) const { - return ( ( current.movementFlags & PMF_JUMPED ) != 0 ); +bool idPhysics_Player::HasJumped(void) const { + return ((current.movementFlags & PMF_JUMPED) != 0); } /* @@ -1459,8 +1548,8 @@ bool idPhysics_Player::HasJumped( void ) const { idPhysics_Player::HasSteppedUp ================ */ -bool idPhysics_Player::HasSteppedUp( void ) const { - return ( ( current.movementFlags & ( PMF_STEPPED_UP | PMF_STEPPED_DOWN ) ) != 0 ); +bool idPhysics_Player::HasSteppedUp(void) const { + return ((current.movementFlags & (PMF_STEPPED_UP | PMF_STEPPED_DOWN)) != 0); } /* @@ -1468,7 +1557,7 @@ bool idPhysics_Player::HasSteppedUp( void ) const { idPhysics_Player::GetStepUp ================ */ -float idPhysics_Player::GetStepUp( void ) const { +float idPhysics_Player::GetStepUp(void) const { return current.stepUp; } @@ -1477,8 +1566,8 @@ float idPhysics_Player::GetStepUp( void ) const { idPhysics_Player::IsCrouching ================ */ -bool idPhysics_Player::IsCrouching( void ) const { - return ( ( current.movementFlags & PMF_DUCKED ) != 0 ); +bool idPhysics_Player::IsCrouching(void) const { + return ((current.movementFlags & PMF_DUCKED) != 0); } /* @@ -1486,7 +1575,7 @@ bool idPhysics_Player::IsCrouching( void ) const { idPhysics_Player::OnLadder ================ */ -bool idPhysics_Player::OnLadder( void ) const { +bool idPhysics_Player::OnLadder(void) const { return ladder; } @@ -1495,17 +1584,17 @@ bool idPhysics_Player::OnLadder( void ) const { idPhysics_Player::idPhysics_Player ================ */ -idPhysics_Player::idPhysics_Player( void ) { +idPhysics_Player::idPhysics_Player(void) { debugLevel = false; clipModel = NULL; clipMask = 0; - memset( ¤t, 0, sizeof( current ) ); + memset(¤t, 0, sizeof(current)); saved = current; walkSpeed = 0; crouchSpeed = 0; maxStepHeight = 0; maxJumpHeight = 0; - memset( &command, 0, sizeof( command ) ); + memset(&command, 0, sizeof(command)); viewAngles.Zero(); framemsec = 0; frametime = 0; @@ -1514,7 +1603,7 @@ idPhysics_Player::idPhysics_Player( void ) { viewRight.Zero(); walking = false; groundPlane = false; - memset( &groundTrace, 0, sizeof( groundTrace ) ); + memset(&groundTrace, 0, sizeof(groundTrace)); groundMaterial = NULL; ladder = false; ladderNormal.Zero(); @@ -1527,15 +1616,15 @@ idPhysics_Player::idPhysics_Player( void ) { idPhysics_Player_SavePState ================ */ -void idPhysics_Player_SavePState( idSaveGame *savefile, const playerPState_t &state ) { - savefile->WriteVec3( state.origin ); - savefile->WriteVec3( state.velocity ); - savefile->WriteVec3( state.localOrigin ); - savefile->WriteVec3( state.pushVelocity ); - savefile->WriteFloat( state.stepUp ); - savefile->WriteInt( state.movementType ); - savefile->WriteInt( state.movementFlags ); - savefile->WriteInt( state.movementTime ); +void idPhysics_Player_SavePState(idSaveGame* savefile, const playerPState_t& state) { + savefile->WriteVec3(state.origin); + savefile->WriteVec3(state.velocity); + savefile->WriteVec3(state.localOrigin); + savefile->WriteVec3(state.pushVelocity); + savefile->WriteFloat(state.stepUp); + savefile->WriteInt(state.movementType); + savefile->WriteInt(state.movementFlags); + savefile->WriteInt(state.movementTime); } /* @@ -1543,15 +1632,15 @@ void idPhysics_Player_SavePState( idSaveGame *savefile, const playerPState_t &st idPhysics_Player_RestorePState ================ */ -void idPhysics_Player_RestorePState( idRestoreGame *savefile, playerPState_t &state ) { - savefile->ReadVec3( state.origin ); - savefile->ReadVec3( state.velocity ); - savefile->ReadVec3( state.localOrigin ); - savefile->ReadVec3( state.pushVelocity ); - savefile->ReadFloat( state.stepUp ); - savefile->ReadInt( state.movementType ); - savefile->ReadInt( state.movementFlags ); - savefile->ReadInt( state.movementTime ); +void idPhysics_Player_RestorePState(idRestoreGame* savefile, playerPState_t& state) { + savefile->ReadVec3(state.origin); + savefile->ReadVec3(state.velocity); + savefile->ReadVec3(state.localOrigin); + savefile->ReadVec3(state.pushVelocity); + savefile->ReadFloat(state.stepUp); + savefile->ReadInt(state.movementType); + savefile->ReadInt(state.movementFlags); + savefile->ReadInt(state.movementTime); } /* @@ -1559,36 +1648,36 @@ void idPhysics_Player_RestorePState( idRestoreGame *savefile, playerPState_t &st idPhysics_Player::Save ================ */ -void idPhysics_Player::Save( idSaveGame *savefile ) const { +void idPhysics_Player::Save(idSaveGame* savefile) const { - idPhysics_Player_SavePState( savefile, current ); - idPhysics_Player_SavePState( savefile, saved ); + idPhysics_Player_SavePState(savefile, current); + idPhysics_Player_SavePState(savefile, saved); - savefile->WriteFloat( walkSpeed ); - savefile->WriteFloat( crouchSpeed ); - savefile->WriteFloat( maxStepHeight ); - savefile->WriteFloat( maxJumpHeight ); - savefile->WriteInt( debugLevel ); + savefile->WriteFloat(walkSpeed); + savefile->WriteFloat(crouchSpeed); + savefile->WriteFloat(maxStepHeight); + savefile->WriteFloat(maxJumpHeight); + savefile->WriteInt(debugLevel); - savefile->WriteUsercmd( command ); - savefile->WriteAngles( viewAngles ); + savefile->WriteUsercmd(command); + savefile->WriteAngles(viewAngles); - savefile->WriteInt( framemsec ); - savefile->WriteFloat( frametime ); - savefile->WriteFloat( playerSpeed ); - savefile->WriteVec3( viewForward ); - savefile->WriteVec3( viewRight ); + savefile->WriteInt(framemsec); + savefile->WriteFloat(frametime); + savefile->WriteFloat(playerSpeed); + savefile->WriteVec3(viewForward); + savefile->WriteVec3(viewRight); - savefile->WriteBool( walking ); - savefile->WriteBool( groundPlane ); - savefile->WriteTrace( groundTrace ); - savefile->WriteMaterial( groundMaterial ); + savefile->WriteBool(walking); + savefile->WriteBool(groundPlane); + savefile->WriteTrace(groundTrace); + savefile->WriteMaterial(groundMaterial); - savefile->WriteBool( ladder ); - savefile->WriteVec3( ladderNormal ); + savefile->WriteBool(ladder); + savefile->WriteVec3(ladderNormal); - savefile->WriteInt( (int)waterLevel ); - savefile->WriteInt( waterType ); + savefile->WriteInt((int)waterLevel); + savefile->WriteInt(waterType); } /* @@ -1596,36 +1685,36 @@ void idPhysics_Player::Save( idSaveGame *savefile ) const { idPhysics_Player::Restore ================ */ -void idPhysics_Player::Restore( idRestoreGame *savefile ) { +void idPhysics_Player::Restore(idRestoreGame* savefile) { - idPhysics_Player_RestorePState( savefile, current ); - idPhysics_Player_RestorePState( savefile, saved ); + idPhysics_Player_RestorePState(savefile, current); + idPhysics_Player_RestorePState(savefile, saved); - savefile->ReadFloat( walkSpeed ); - savefile->ReadFloat( crouchSpeed ); - savefile->ReadFloat( maxStepHeight ); - savefile->ReadFloat( maxJumpHeight ); - savefile->ReadInt( debugLevel ); + savefile->ReadFloat(walkSpeed); + savefile->ReadFloat(crouchSpeed); + savefile->ReadFloat(maxStepHeight); + savefile->ReadFloat(maxJumpHeight); + savefile->ReadInt(debugLevel); - savefile->ReadUsercmd( command ); - savefile->ReadAngles( viewAngles ); + savefile->ReadUsercmd(command); + savefile->ReadAngles(viewAngles); - savefile->ReadInt( framemsec ); - savefile->ReadFloat( frametime ); - savefile->ReadFloat( playerSpeed ); - savefile->ReadVec3( viewForward ); - savefile->ReadVec3( viewRight ); + savefile->ReadInt(framemsec); + savefile->ReadFloat(frametime); + savefile->ReadFloat(playerSpeed); + savefile->ReadVec3(viewForward); + savefile->ReadVec3(viewRight); - savefile->ReadBool( walking ); - savefile->ReadBool( groundPlane ); - savefile->ReadTrace( groundTrace ); - savefile->ReadMaterial( groundMaterial ); + savefile->ReadBool(walking); + savefile->ReadBool(groundPlane); + savefile->ReadTrace(groundTrace); + savefile->ReadMaterial(groundMaterial); - savefile->ReadBool( ladder ); - savefile->ReadVec3( ladderNormal ); + savefile->ReadBool(ladder); + savefile->ReadVec3(ladderNormal); - savefile->ReadInt( (int &)waterLevel ); - savefile->ReadInt( waterType ); + savefile->ReadInt((int&)waterLevel); + savefile->ReadInt(waterType); } /* @@ -1633,7 +1722,7 @@ void idPhysics_Player::Restore( idRestoreGame *savefile ) { idPhysics_Player::SetPlayerInput ================ */ -void idPhysics_Player::SetPlayerInput( const usercmd_t &cmd, const idAngles &newViewAngles ) { +void idPhysics_Player::SetPlayerInput(const usercmd_t& cmd, const idAngles& newViewAngles) { command = cmd; viewAngles = newViewAngles; // can't use cmd.angles cause of the delta_angles } @@ -1643,7 +1732,7 @@ void idPhysics_Player::SetPlayerInput( const usercmd_t &cmd, const idAngles &new idPhysics_Player::SetSpeed ================ */ -void idPhysics_Player::SetSpeed( const float newWalkSpeed, const float newCrouchSpeed ) { +void idPhysics_Player::SetSpeed(const float newWalkSpeed, const float newCrouchSpeed) { walkSpeed = newWalkSpeed; crouchSpeed = newCrouchSpeed; } @@ -1653,7 +1742,7 @@ void idPhysics_Player::SetSpeed( const float newWalkSpeed, const float newCrouch idPhysics_Player::SetMaxStepHeight ================ */ -void idPhysics_Player::SetMaxStepHeight( const float newMaxStepHeight ) { +void idPhysics_Player::SetMaxStepHeight(const float newMaxStepHeight) { maxStepHeight = newMaxStepHeight; } @@ -1662,7 +1751,7 @@ void idPhysics_Player::SetMaxStepHeight( const float newMaxStepHeight ) { idPhysics_Player::GetMaxStepHeight ================ */ -float idPhysics_Player::GetMaxStepHeight( void ) const { +float idPhysics_Player::GetMaxStepHeight(void) const { return maxStepHeight; } @@ -1671,7 +1760,7 @@ float idPhysics_Player::GetMaxStepHeight( void ) const { idPhysics_Player::SetMaxJumpHeight ================ */ -void idPhysics_Player::SetMaxJumpHeight( const float newMaxJumpHeight ) { +void idPhysics_Player::SetMaxJumpHeight(const float newMaxJumpHeight) { maxJumpHeight = newMaxJumpHeight; } @@ -1680,7 +1769,7 @@ void idPhysics_Player::SetMaxJumpHeight( const float newMaxJumpHeight ) { idPhysics_Player::SetMovementType ================ */ -void idPhysics_Player::SetMovementType( const pmtype_t type ) { +void idPhysics_Player::SetMovementType(const pmtype_t type) { current.movementType = type; } @@ -1689,8 +1778,8 @@ void idPhysics_Player::SetMovementType( const pmtype_t type ) { idPhysics_Player::SetKnockBack ================ */ -void idPhysics_Player::SetKnockBack( const int knockBackTime ) { - if ( current.movementTime ) { +void idPhysics_Player::SetKnockBack(const int knockBackTime) { + if (current.movementTime) { return; } current.movementFlags |= PMF_TIME_KNOCKBACK; @@ -1702,7 +1791,7 @@ void idPhysics_Player::SetKnockBack( const int knockBackTime ) { idPhysics_Player::SetDebugLevel ================ */ -void idPhysics_Player::SetDebugLevel( bool set ) { +void idPhysics_Player::SetDebugLevel(bool set) { debugLevel = set; } @@ -1711,10 +1800,16 @@ void idPhysics_Player::SetDebugLevel( bool set ) { idPhysics_Player::Evaluate ================ */ -bool idPhysics_Player::Evaluate( int timeStepMSec, int endTimeMSec ) { +bool idPhysics_Player::Evaluate(int timeStepMSec, int endTimeMSec) { idVec3 masterOrigin, oldOrigin; idMat3 masterAxis; + // eliminate tiny values that can otherwise create denormals in long-running physics simulations + current.velocity.FixDenormals(); + current.pushVelocity.FixDenormals(); + current.origin.FixDenormals(); + current.localOrigin.FixDenormals(); + waterLevel = WATERLEVEL_NONE; waterType = 0; oldOrigin = current.origin; @@ -1722,11 +1817,13 @@ bool idPhysics_Player::Evaluate( int timeStepMSec, int endTimeMSec ) { clipModel->Unlink(); // if bound to a master - if ( masterEntity ) { - self->GetMasterPosition( masterOrigin, masterAxis ); + if (masterEntity) { + self->GetMasterPosition(masterOrigin, masterAxis); current.origin = masterOrigin + current.localOrigin * masterAxis; - clipModel->Link( gameLocal.clip, self, 0, current.origin, clipModel->GetAxis() ); - current.velocity = ( current.origin - oldOrigin ) / ( timeStepMSec * 0.001f ); + clipModel->Link(gameLocal.clip, self, 0, current.origin, clipModel->GetAxis()); + if (timeStepMSec > 0) { + current.velocity = (current.origin - oldOrigin) / (timeStepMSec * 0.001f); + } masterDeltaYaw = masterYaw; masterYaw = masterAxis[0].ToYaw(); masterDeltaYaw = masterYaw - masterDeltaYaw; @@ -1735,12 +1832,12 @@ bool idPhysics_Player::Evaluate( int timeStepMSec, int endTimeMSec ) { ActivateContactEntities(); - idPhysics_Player::MovePlayer( timeStepMSec ); + idPhysics_Player::MovePlayer(timeStepMSec); - clipModel->Link( gameLocal.clip, self, 0, current.origin, clipModel->GetAxis() ); + clipModel->Link(gameLocal.clip, self, 0, current.origin, clipModel->GetAxis()); - if ( IsOutsideWorld() ) { - gameLocal.Warning( "clip model outside world bounds for entity '%s' at (%s)", self->name.c_str(), current.origin.ToString(0) ); + if (IsOutsideWorld()) { + gameLocal.Warning("clip model outside world bounds for entity '%s' at (%s)", self->name.c_str(), current.origin.ToString(0)); } return true; //( current.origin != oldOrigin ); @@ -1751,7 +1848,7 @@ bool idPhysics_Player::Evaluate( int timeStepMSec, int endTimeMSec ) { idPhysics_Player::UpdateTime ================ */ -void idPhysics_Player::UpdateTime( int endTimeMSec ) { +void idPhysics_Player::UpdateTime(int endTimeMSec) { } /* @@ -1759,7 +1856,7 @@ void idPhysics_Player::UpdateTime( int endTimeMSec ) { idPhysics_Player::GetTime ================ */ -int idPhysics_Player::GetTime( void ) const { +int idPhysics_Player::GetTime(void) const { return gameLocal.time; } @@ -1768,7 +1865,7 @@ int idPhysics_Player::GetTime( void ) const { idPhysics_Player::GetImpactInfo ================ */ -void idPhysics_Player::GetImpactInfo( const int id, const idVec3 &point, impactInfo_t *info ) const { +void idPhysics_Player::GetImpactInfo(const int id, const idVec3& point, impactInfo_t* info) const { info->invMass = invMass; info->invInertiaTensor.Zero(); info->position.Zero(); @@ -1780,9 +1877,13 @@ void idPhysics_Player::GetImpactInfo( const int id, const idVec3 &point, impactI idPhysics_Player::ApplyImpulse ================ */ -void idPhysics_Player::ApplyImpulse( const int id, const idVec3 &point, const idVec3 &impulse ) { - if ( current.movementType != PM_NOCLIP ) { - current.velocity += impulse * invMass; +void idPhysics_Player::ApplyImpulse(const int id, const idVec3& point, const idVec3& impulse) { + if (current.movementType != PM_NOCLIP) { + idVec3 add = impulse * invMass; + if (groundPlane) { + add -= (add * groundTrace.c.normal) * groundTrace.c.normal; + } + current.velocity += add; } } @@ -1791,7 +1892,7 @@ void idPhysics_Player::ApplyImpulse( const int id, const idVec3 &point, const id idPhysics_Player::IsAtRest ================ */ -bool idPhysics_Player::IsAtRest( void ) const { +bool idPhysics_Player::IsAtRest(void) const { return false; } @@ -1800,7 +1901,7 @@ bool idPhysics_Player::IsAtRest( void ) const { idPhysics_Player::GetRestStartTime ================ */ -int idPhysics_Player::GetRestStartTime( void ) const { +int idPhysics_Player::GetRestStartTime(void) const { return -1; } @@ -1809,7 +1910,7 @@ int idPhysics_Player::GetRestStartTime( void ) const { idPhysics_Player::SaveState ================ */ -void idPhysics_Player::SaveState( void ) { +void idPhysics_Player::SaveState(void) { saved = current; } @@ -1818,10 +1919,10 @@ void idPhysics_Player::SaveState( void ) { idPhysics_Player::RestoreState ================ */ -void idPhysics_Player::RestoreState( void ) { +void idPhysics_Player::RestoreState(void) { current = saved; - clipModel->Link( gameLocal.clip, self, 0, current.origin, clipModel->GetAxis() ); + clipModel->Link(gameLocal.clip, self, 0, current.origin, clipModel->GetAxis()); EvaluateContacts(); } @@ -1831,20 +1932,20 @@ void idPhysics_Player::RestoreState( void ) { idPhysics_Player::SetOrigin ================ */ -void idPhysics_Player::SetOrigin( const idVec3 &newOrigin, int id ) { +void idPhysics_Player::SetOrigin(const idVec3& newOrigin, int id) { idVec3 masterOrigin; idMat3 masterAxis; current.localOrigin = newOrigin; - if ( masterEntity ) { - self->GetMasterPosition( masterOrigin, masterAxis ); + if (masterEntity) { + self->GetMasterPosition(masterOrigin, masterAxis); current.origin = masterOrigin + newOrigin * masterAxis; } else { current.origin = newOrigin; } - clipModel->Link( gameLocal.clip, self, 0, newOrigin, clipModel->GetAxis() ); + clipModel->Link(gameLocal.clip, self, 0, current.origin, clipModel->GetAxis()); } /* @@ -1852,7 +1953,7 @@ void idPhysics_Player::SetOrigin( const idVec3 &newOrigin, int id ) { idPhysics_Player::GetOrigin ================ */ -const idVec3 & idPhysics_Player::PlayerGetOrigin( void ) const { +const idVec3& idPhysics_Player::PlayerGetOrigin(void) const { return current.origin; } @@ -1861,8 +1962,8 @@ const idVec3 & idPhysics_Player::PlayerGetOrigin( void ) const { idPhysics_Player::SetAxis ================ */ -void idPhysics_Player::SetAxis( const idMat3 &newAxis, int id ) { - clipModel->Link( gameLocal.clip, self, 0, clipModel->GetOrigin(), newAxis ); +void idPhysics_Player::SetAxis(const idMat3& newAxis, int id) { + clipModel->Link(gameLocal.clip, self, 0, current.origin, newAxis); } /* @@ -1870,12 +1971,12 @@ void idPhysics_Player::SetAxis( const idMat3 &newAxis, int id ) { idPhysics_Player::Translate ================ */ -void idPhysics_Player::Translate( const idVec3 &translation, int id ) { +void idPhysics_Player::Translate(const idVec3& translation, int id) { current.localOrigin += translation; current.origin += translation; - clipModel->Link( gameLocal.clip, self, 0, current.origin, clipModel->GetAxis() ); + clipModel->Link(gameLocal.clip, self, 0, current.origin, clipModel->GetAxis()); } /* @@ -1883,20 +1984,20 @@ void idPhysics_Player::Translate( const idVec3 &translation, int id ) { idPhysics_Player::Rotate ================ */ -void idPhysics_Player::Rotate( const idRotation &rotation, int id ) { +void idPhysics_Player::Rotate(const idRotation& rotation, int id) { idVec3 masterOrigin; idMat3 masterAxis; current.origin *= rotation; - if ( masterEntity ) { - self->GetMasterPosition( masterOrigin, masterAxis ); - current.localOrigin = ( current.origin - masterOrigin ) * masterAxis.Transpose(); + if (masterEntity) { + self->GetMasterPosition(masterOrigin, masterAxis); + current.localOrigin = (current.origin - masterOrigin) * masterAxis.Transpose(); } else { current.localOrigin = current.origin; } - clipModel->Link( gameLocal.clip, self, 0, current.origin, clipModel->GetAxis() * rotation.ToMat3() ); + clipModel->Link(gameLocal.clip, self, 0, current.origin, clipModel->GetAxis() * rotation.ToMat3()); } /* @@ -1904,7 +2005,7 @@ void idPhysics_Player::Rotate( const idRotation &rotation, int id ) { idPhysics_Player::SetLinearVelocity ================ */ -void idPhysics_Player::SetLinearVelocity( const idVec3 &newLinearVelocity, int id ) { +void idPhysics_Player::SetLinearVelocity(const idVec3& newLinearVelocity, int id) { current.velocity = newLinearVelocity; } @@ -1913,7 +2014,7 @@ void idPhysics_Player::SetLinearVelocity( const idVec3 &newLinearVelocity, int i idPhysics_Player::GetLinearVelocity ================ */ -const idVec3 &idPhysics_Player::GetLinearVelocity( int id ) const { +const idVec3& idPhysics_Player::GetLinearVelocity(int id) const { return current.velocity; } @@ -1922,16 +2023,16 @@ const idVec3 &idPhysics_Player::GetLinearVelocity( int id ) const { idPhysics_Player::SetPushed ================ */ -void idPhysics_Player::SetPushed( int deltaTime ) { +void idPhysics_Player::SetPushed(int deltaTime) { idVec3 velocity; float d; // velocity with which the player is pushed - velocity = ( current.origin - saved.origin ) / ( deltaTime * idMath::M_MS2SEC ); + velocity = (current.origin - saved.origin) / (deltaTime * idMath::M_MS2SEC); // remove any downward push velocity d = velocity * gravityNormal; - if ( d > 0.0f ) { + if (d > 0.0f) { velocity -= d * gravityNormal; } @@ -1943,7 +2044,7 @@ void idPhysics_Player::SetPushed( int deltaTime ) { idPhysics_Player::GetPushedLinearVelocity ================ */ -const idVec3 &idPhysics_Player::GetPushedLinearVelocity( const int id ) const { +const idVec3& idPhysics_Player::GetPushedLinearVelocity(const int id) const { return current.pushVelocity; } @@ -1952,7 +2053,7 @@ const idVec3 &idPhysics_Player::GetPushedLinearVelocity( const int id ) const { idPhysics_Player::ClearPushedVelocity ================ */ -void idPhysics_Player::ClearPushedVelocity( void ) { +void idPhysics_Player::ClearPushedVelocity(void) { current.pushVelocity.Zero(); } @@ -1963,56 +2064,56 @@ idPhysics_Player::SetMaster the binding is never orientated ================ */ -void idPhysics_Player::SetMaster( idEntity *master, const bool orientated ) { +void idPhysics_Player::SetMaster(idEntity* master, const bool orientated) { idVec3 masterOrigin; idMat3 masterAxis; - if ( master ) { - if ( !masterEntity ) { + if (master) { + if (!masterEntity) { // transform from world space to master space - self->GetMasterPosition( masterOrigin, masterAxis ); - current.localOrigin = ( current.origin - masterOrigin ) * masterAxis.Transpose(); + self->GetMasterPosition(masterOrigin, masterAxis); + current.localOrigin = (current.origin - masterOrigin) * masterAxis.Transpose(); masterEntity = master; masterYaw = masterAxis[0].ToYaw(); } ClearContacts(); } else { - if ( masterEntity ) { + if (masterEntity) { masterEntity = NULL; } } } -const float PLAYER_VELOCITY_MAX = 4000; -const int PLAYER_VELOCITY_TOTAL_BITS = 16; -const int PLAYER_VELOCITY_EXPONENT_BITS = idMath::BitsForInteger( idMath::BitsForFloat( PLAYER_VELOCITY_MAX ) ) + 1; -const int PLAYER_VELOCITY_MANTISSA_BITS = PLAYER_VELOCITY_TOTAL_BITS - 1 - PLAYER_VELOCITY_EXPONENT_BITS; -const int PLAYER_MOVEMENT_TYPE_BITS = 3; -const int PLAYER_MOVEMENT_FLAGS_BITS = 8; +const float PLAYER_VELOCITY_MAX = 4000; +const int PLAYER_VELOCITY_TOTAL_BITS = 16; +const int PLAYER_VELOCITY_EXPONENT_BITS = idMath::BitsForInteger(idMath::BitsForFloat(PLAYER_VELOCITY_MAX)) + 1; +const int PLAYER_VELOCITY_MANTISSA_BITS = PLAYER_VELOCITY_TOTAL_BITS - 1 - PLAYER_VELOCITY_EXPONENT_BITS; +const int PLAYER_MOVEMENT_TYPE_BITS = 3; +const int PLAYER_MOVEMENT_FLAGS_BITS = 8; /* ================ idPhysics_Player::WriteToSnapshot ================ */ -void idPhysics_Player::WriteToSnapshot( idBitMsgDelta &msg ) const { - msg.WriteFloat( current.origin[0] ); - msg.WriteFloat( current.origin[1] ); - msg.WriteFloat( current.origin[2] ); - msg.WriteFloat( current.velocity[0], PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS ); - msg.WriteFloat( current.velocity[1], PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS ); - msg.WriteFloat( current.velocity[2], PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( current.origin[0], current.localOrigin[0] ); - msg.WriteDeltaFloat( current.origin[1], current.localOrigin[1] ); - msg.WriteDeltaFloat( current.origin[2], current.localOrigin[2] ); - msg.WriteDeltaFloat( 0.0f, current.pushVelocity[0], PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, current.pushVelocity[1], PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, current.pushVelocity[2], PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, current.stepUp ); - msg.WriteBits( current.movementType, PLAYER_MOVEMENT_TYPE_BITS ); - msg.WriteBits( current.movementFlags, PLAYER_MOVEMENT_FLAGS_BITS ); - msg.WriteDeltaLong( 0, current.movementTime ); +void idPhysics_Player::WriteToSnapshot(idBitMsgDelta& msg) const { + msg.WriteFloat(current.origin[0]); + msg.WriteFloat(current.origin[1]); + msg.WriteFloat(current.origin[2]); + msg.WriteFloat(current.velocity[0], PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS); + msg.WriteFloat(current.velocity[1], PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS); + msg.WriteFloat(current.velocity[2], PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(current.origin[0], current.localOrigin[0]); + msg.WriteDeltaFloat(current.origin[1], current.localOrigin[1]); + msg.WriteDeltaFloat(current.origin[2], current.localOrigin[2]); + msg.WriteDeltaFloat(0.0f, current.pushVelocity[0], PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, current.pushVelocity[1], PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, current.pushVelocity[2], PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, current.stepUp); + msg.WriteBits(current.movementType, PLAYER_MOVEMENT_TYPE_BITS); + msg.WriteBits(current.movementFlags, PLAYER_MOVEMENT_FLAGS_BITS); + msg.WriteDeltaLong(0, current.movementTime); } /* @@ -2020,26 +2121,31 @@ void idPhysics_Player::WriteToSnapshot( idBitMsgDelta &msg ) const { idPhysics_Player::ReadFromSnapshot ================ */ -void idPhysics_Player::ReadFromSnapshot( const idBitMsgDelta &msg ) { +void idPhysics_Player::ReadFromSnapshot(const idBitMsgDelta& msg) { current.origin[0] = msg.ReadFloat(); current.origin[1] = msg.ReadFloat(); current.origin[2] = msg.ReadFloat(); - current.velocity[0] = msg.ReadFloat( PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS ); - current.velocity[1] = msg.ReadFloat( PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS ); - current.velocity[2] = msg.ReadFloat( PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS ); - current.localOrigin[0] = msg.ReadDeltaFloat( current.origin[0] ); - current.localOrigin[1] = msg.ReadDeltaFloat( current.origin[1] ); - current.localOrigin[2] = msg.ReadDeltaFloat( current.origin[2] ); - current.pushVelocity[0] = msg.ReadDeltaFloat( 0.0f, PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS ); - current.pushVelocity[1] = msg.ReadDeltaFloat( 0.0f, PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS ); - current.pushVelocity[2] = msg.ReadDeltaFloat( 0.0f, PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS ); - current.stepUp = msg.ReadDeltaFloat( 0.0f ); - current.movementType = msg.ReadBits( PLAYER_MOVEMENT_TYPE_BITS ); - current.movementFlags = msg.ReadBits( PLAYER_MOVEMENT_FLAGS_BITS ); - current.movementTime = msg.ReadDeltaLong( 0 ); + current.velocity[0] = msg.ReadFloat(PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS); + current.velocity[1] = msg.ReadFloat(PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS); + current.velocity[2] = msg.ReadFloat(PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS); + current.localOrigin[0] = msg.ReadDeltaFloat(current.origin[0]); + current.localOrigin[1] = msg.ReadDeltaFloat(current.origin[1]); + current.localOrigin[2] = msg.ReadDeltaFloat(current.origin[2]); + current.pushVelocity[0] = msg.ReadDeltaFloat(0.0f, PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS); + current.pushVelocity[1] = msg.ReadDeltaFloat(0.0f, PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS); + current.pushVelocity[2] = msg.ReadDeltaFloat(0.0f, PLAYER_VELOCITY_EXPONENT_BITS, PLAYER_VELOCITY_MANTISSA_BITS); + current.stepUp = msg.ReadDeltaFloat(0.0f); + current.movementType = msg.ReadBits(PLAYER_MOVEMENT_TYPE_BITS); + current.movementFlags = msg.ReadBits(PLAYER_MOVEMENT_FLAGS_BITS); + current.movementTime = msg.ReadDeltaLong(0); - if ( clipModel ) { - clipModel->Link( gameLocal.clip, self, 0, current.origin, clipModel->GetAxis() ); + current.origin.FixDenormals(); + current.velocity.FixDenormals(); + current.localOrigin.FixDenormals(); + current.pushVelocity.FixDenormals(); + + if (clipModel) { + clipModel->Link(gameLocal.clip, self, 0, current.origin, clipModel->GetAxis()); } } diff --git a/neo/game/physics/Physics_RigidBody.cpp b/neo/game/physics/Physics_RigidBody.cpp index f0e8bbac..02c331c9 100644 --- a/neo/game/physics/Physics_RigidBody.cpp +++ b/neo/game/physics/Physics_RigidBody.cpp @@ -2,9 +2,9 @@ =========================================================================== Doom 3 GPL Source Code -Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. +Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company. -This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?). +This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?). Doom 3 Source Code is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by @@ -31,10 +31,10 @@ If you have questions concerning this license or the applicable additional terms #include "../Game_local.h" -CLASS_DECLARATION( idPhysics_Base, idPhysics_RigidBody ) +CLASS_DECLARATION(idPhysics_Base, idPhysics_RigidBody) END_CLASS -const float STOP_SPEED = 10.0f; +const float STOP_SPEED = 10.0f; #undef RB_TIMINGS @@ -51,16 +51,16 @@ static idTimer timer_total, timer_collision; RigidBodyDerivatives ================ */ -void RigidBodyDerivatives( const float t, const void *clientData, const float *state, float *derivatives ) { - const idPhysics_RigidBody *p = (idPhysics_RigidBody *) clientData; - rigidBodyIState_t *s = (rigidBodyIState_t *) state; +void RigidBodyDerivatives(const float t, const void* clientData, const float* state, float* derivatives) { + const idPhysics_RigidBody* p = (idPhysics_RigidBody*)clientData; + rigidBodyIState_t* s = (rigidBodyIState_t*)state; // NOTE: this struct should be build conform rigidBodyIState_t struct rigidBodyDerivatives_s { idVec3 linearVelocity; idMat3 angularMatrix; idVec3 force; idVec3 torque; - } *d = (struct rigidBodyDerivatives_s *) derivatives; + } *d = (struct rigidBodyDerivatives_s*)derivatives; idVec3 angularVelocity; idMat3 inverseWorldInertiaTensor; @@ -68,9 +68,9 @@ void RigidBodyDerivatives( const float t, const void *clientData, const float *s angularVelocity = inverseWorldInertiaTensor * s->angularMomentum; // derivatives d->linearVelocity = p->inverseMass * s->linearMomentum; - d->angularMatrix = SkewSymmetric( angularVelocity ) * s->orientation; - d->force = - p->linearFriction * s->linearMomentum + p->current.externalForce; - d->torque = - p->angularFriction * s->angularMomentum + p->current.externalTorque; + d->angularMatrix = SkewSymmetric(angularVelocity) * s->orientation; + d->force = -p->linearFriction * s->linearMomentum + p->current.externalForce; + d->torque = -p->angularFriction * s->angularMomentum + p->current.externalTorque; } /* @@ -80,7 +80,7 @@ idPhysics_RigidBody::Integrate Calculate next state from the current state using an integrator. ================ */ -void idPhysics_RigidBody::Integrate( float deltaTime, rigidBodyPState_t &next ) { +void idPhysics_RigidBody::Integrate(float deltaTime, rigidBodyPState_t& next) { idVec3 position; position = current.i.position; @@ -88,7 +88,7 @@ void idPhysics_RigidBody::Integrate( float deltaTime, rigidBodyPState_t &next ) current.i.orientation.TransposeSelf(); - integrator->Evaluate( (float *) ¤t.i, (float *) &next.i, 0, deltaTime ); + integrator->Evaluate((float*)¤t.i, (float*)&next.i, 0, deltaTime); next.i.orientation.OrthoNormalizeSelf(); // apply gravity @@ -111,19 +111,19 @@ idPhysics_RigidBody::CollisionImpulse The current state should be set to the moment of impact. ================ */ -bool idPhysics_RigidBody::CollisionImpulse( const trace_t &collision, idVec3 &impulse ) { +bool idPhysics_RigidBody::CollisionImpulse(const trace_t& collision, idVec3& impulse) { idVec3 r, linearVelocity, angularVelocity, velocity; idMat3 inverseWorldInertiaTensor; float impulseNumerator, impulseDenominator, vel; impactInfo_t info; - idEntity *ent; + idEntity* ent; // get info from other entity involved ent = gameLocal.entities[collision.c.entityNum]; - ent->GetImpactInfo( self, collision.c.id, collision.c.point, &info ); + ent->GetImpactInfo(self, collision.c.id, collision.c.point, &info); // collision point relative to the body center of mass - r = collision.c.point - ( current.i.position + centerOfMass * current.i.orientation ); + r = collision.c.point - (current.i.position + centerOfMass * current.i.orientation); // the velocity at the collision point linearVelocity = inverseMass * current.i.linearMomentum; inverseWorldInertiaTensor = current.i.orientation.Transpose() * inverseInertiaTensor * current.i.orientation; @@ -135,15 +135,15 @@ bool idPhysics_RigidBody::CollisionImpulse( const trace_t &collision, idVec3 &im // velocity in normal direction vel = velocity * collision.c.normal; - if ( vel > -STOP_SPEED ) { + if (vel > -STOP_SPEED) { impulseNumerator = STOP_SPEED; } else { - impulseNumerator = -( 1.0f + bouncyness ) * vel; + impulseNumerator = -(1.0f + bouncyness) * vel; } - impulseDenominator = inverseMass + ( ( inverseWorldInertiaTensor * r.Cross( collision.c.normal ) ).Cross( r ) * collision.c.normal ); - if ( info.invMass ) { - impulseDenominator += info.invMass + ( ( info.invInertiaTensor * info.position.Cross( collision.c.normal ) ).Cross( info.position ) * collision.c.normal ); + impulseDenominator = inverseMass + ((inverseWorldInertiaTensor * r.Cross(collision.c.normal)).Cross(r) * collision.c.normal); + if (info.invMass) { + impulseDenominator += info.invMass + ((info.invInertiaTensor * info.position.Cross(collision.c.normal)).Cross(info.position) * collision.c.normal); } impulse = (impulseNumerator / impulseDenominator) * collision.c.normal; @@ -152,13 +152,13 @@ bool idPhysics_RigidBody::CollisionImpulse( const trace_t &collision, idVec3 &im current.i.angularMomentum += r.Cross(impulse); // if no movement at all don't blow up - if ( collision.fraction < 0.0001f ) { + if (collision.fraction < 0.0001f) { current.i.linearMomentum *= 0.5f; current.i.angularMomentum *= 0.5f; } // callback to self to let the entity know about the collision - return self->Collide( collision, velocity ); + return self->Collide(collision, velocity); } /* @@ -169,25 +169,26 @@ idPhysics_RigidBody::CheckForCollisions If there is a collision the next state is set to the state at the moment of impact. ================ */ -bool idPhysics_RigidBody::CheckForCollisions( const float deltaTime, rigidBodyPState_t &next, trace_t &collision ) { -//#define TEST_COLLISION_DETECTION +bool idPhysics_RigidBody::CheckForCollisions(const float deltaTime, rigidBodyPState_t& next, trace_t& collision) { + //#define TEST_COLLISION_DETECTION idMat3 axis; idRotation rotation; bool collided = false; + collision.fraction = 1.0f; #ifdef TEST_COLLISION_DETECTION - bool startsolid; - if ( gameLocal.clip.Contents( current.i.position, clipModel, current.i.orientation, clipMask, self ) ) { + bool startsolid = false; + if (gameLocal.clip.Contents(current.i.position, clipModel, current.i.orientation, clipMask, self)) { startsolid = true; } #endif - TransposeMultiply( current.i.orientation, next.i.orientation, axis ); + TransposeMultiply(current.i.orientation, next.i.orientation, axis); rotation = axis.ToRotation(); - rotation.SetOrigin( current.i.position ); + rotation.SetOrigin(current.i.position); // if there was a collision - if ( gameLocal.clip.Motion( collision, current.i.position, next.i.position, rotation, clipModel, current.i.orientation, clipMask, self ) ) { + if (gameLocal.clip.Motion(collision, current.i.position, next.i.position, rotation, clipModel, current.i.orientation, clipMask, self)) { // set the next state to the state at the moment of impact next.i.position = collision.endpos; next.i.orientation = collision.endAxis; @@ -197,8 +198,8 @@ bool idPhysics_RigidBody::CheckForCollisions( const float deltaTime, rigidBodyPS } #ifdef TEST_COLLISION_DETECTION - if ( gameLocal.clip.Contents( next.i.position, clipModel, next.i.orientation, clipMask, self ) ) { - if ( !startsolid ) { + if (gameLocal.clip.Contents(next.i.position, clipModel, next.i.orientation, clipMask, self)) { + if (!startsolid) { int bah = 1; } } @@ -214,7 +215,7 @@ idPhysics_RigidBody::ContactFriction Uses absolute velocity at the contact points instead of the velocity relative to the contact object. ================ */ -void idPhysics_RigidBody::ContactFriction( float deltaTime ) { +void idPhysics_RigidBody::ContactFriction(float deltaTime) { int i; float magnitude, impulseNumerator, impulseDenominator; idMat3 inverseWorldInertiaTensor; @@ -225,7 +226,7 @@ void idPhysics_RigidBody::ContactFriction( float deltaTime ) { massCenter = current.i.position + centerOfMass * current.i.orientation; - for ( i = 0; i < contacts.Num(); i++ ) { + for (i = 0; i < contacts.Num(); i++) { r = contacts[i].point - massCenter; @@ -235,13 +236,13 @@ void idPhysics_RigidBody::ContactFriction( float deltaTime ) { velocity = linearVelocity + angularVelocity.Cross(r); // velocity along normal vector - normalVelocity = ( velocity * contacts[i].normal ) * contacts[i].normal; + normalVelocity = (velocity * contacts[i].normal) * contacts[i].normal; // calculate friction impulse - normal = -( velocity - normalVelocity ); + normal = -(velocity - normalVelocity); magnitude = normal.Normalize(); impulseNumerator = contactFriction * magnitude; - impulseDenominator = inverseMass + ( ( inverseWorldInertiaTensor * r.Cross( normal ) ).Cross( r ) * normal ); + impulseDenominator = inverseMass + ((inverseWorldInertiaTensor * r.Cross(normal)).Cross(r) * normal); impulse = (impulseNumerator / impulseDenominator) * normal; // apply friction impulse @@ -249,16 +250,16 @@ void idPhysics_RigidBody::ContactFriction( float deltaTime ) { current.i.angularMomentum += r.Cross(impulse); // if moving towards the surface at the contact point - if ( normalVelocity * contacts[i].normal < 0.0f ) { + if (normalVelocity * contacts[i].normal < 0.0f) { // calculate impulse normal = -normalVelocity; impulseNumerator = normal.Normalize(); - impulseDenominator = inverseMass + ( ( inverseWorldInertiaTensor * r.Cross( normal ) ).Cross( r ) * normal ); + impulseDenominator = inverseMass + ((inverseWorldInertiaTensor * r.Cross(normal)).Cross(r) * normal); impulse = (impulseNumerator / impulseDenominator) * normal; // apply impulse current.i.linearMomentum += impulse; - current.i.angularMomentum += r.Cross( impulse ); + current.i.angularMomentum += r.Cross(impulse); } } } @@ -271,45 +272,45 @@ idPhysics_RigidBody::TestIfAtRest Does not catch all cases where the body is at rest but is generally good enough. ================ */ -bool idPhysics_RigidBody::TestIfAtRest( void ) const { +bool idPhysics_RigidBody::TestIfAtRest(void) const { int i; float gv; idVec3 v, av, normal, point; idMat3 inverseWorldInertiaTensor; idFixedWinding contactWinding; - if ( current.atRest >= 0 ) { + if (current.atRest >= 0) { return true; } // need at least 3 contact points to come to rest - if ( contacts.Num() < 3 ) { + if (contacts.Num() < 3) { return false; } // get average contact plane normal normal.Zero(); - for ( i = 0; i < contacts.Num(); i++ ) { + for (i = 0; i < contacts.Num(); i++) { normal += contacts[i].normal; } - normal /= (float) contacts.Num(); + normal /= (float)contacts.Num(); normal.Normalize(); // if on a too steep surface - if ( (normal * gravityNormal) > -0.7f ) { + if ((normal * gravityNormal) > -0.7f) { return false; } // create bounds for contact points contactWinding.Clear(); - for ( i = 0; i < contacts.Num(); i++ ) { + for (i = 0; i < contacts.Num(); i++) { // project point onto plane through origin orthogonal to the gravity point = contacts[i].point - (contacts[i].point * gravityNormal) * gravityNormal; - contactWinding.AddToConvexHull( point, gravityNormal ); + contactWinding.AddToConvexHull(point, gravityNormal); } // need at least 3 contact points to come to rest - if ( contactWinding.GetNumPoints() < 3 ) { + if (contactWinding.GetNumPoints() < 3) { return false; } @@ -318,7 +319,7 @@ bool idPhysics_RigidBody::TestIfAtRest( void ) const { point -= (point * gravityNormal) * gravityNormal; // if the point is not inside the winding - if ( !contactWinding.PointInside( gravityNormal, point, 0 ) ) { + if (!contactWinding.PointInside(gravityNormal, point, 0)) { return false; } @@ -330,11 +331,11 @@ bool idPhysics_RigidBody::TestIfAtRest( void ) const { v -= gv * gravityNormal; // if too much velocity orthogonal to gravity direction - if ( v.Length() > STOP_SPEED ) { + if (v.Length() > STOP_SPEED) { return false; } // if too much velocity in gravity direction - if ( gv > 2.0f * STOP_SPEED || gv < -2.0f * STOP_SPEED ) { + if (gv > 2.0f * STOP_SPEED || gv < -2.0f * STOP_SPEED) { return false; } @@ -343,7 +344,7 @@ bool idPhysics_RigidBody::TestIfAtRest( void ) const { av = inverseWorldInertiaTensor * current.i.angularMomentum; // if too much rotational velocity - if ( av.LengthSqr() > STOP_SPEED ) { + if (av.LengthSqr() > STOP_SPEED) { return false; } @@ -357,17 +358,17 @@ idPhysics_RigidBody::DropToFloorAndRest Drops the object straight down to the floor and verifies if the object is at rest on the floor. ================ */ -void idPhysics_RigidBody::DropToFloorAndRest( void ) { +void idPhysics_RigidBody::DropToFloorAndRest(void) { idVec3 down; trace_t tr; - if ( testSolid ) { + if (testSolid) { testSolid = false; - if ( gameLocal.clip.Contents( current.i.position, clipModel, current.i.orientation, clipMask, self ) ) { - gameLocal.DWarning( "rigid body in solid for entity '%s' type '%s' at (%s)", - self->name.c_str(), self->GetType()->classname, current.i.position.ToString(0) ); + if (gameLocal.clip.Contents(current.i.position, clipModel, current.i.orientation, clipMask, self)) { + gameLocal.DWarning("rigid body in solid for entity '%s' type '%s' at (%s)", + self->name.c_str(), self->GetType()->classname, current.i.position.ToString(0)); Rest(); dropToFloor = false; return; @@ -376,23 +377,24 @@ void idPhysics_RigidBody::DropToFloorAndRest( void ) { // put the body on the floor down = current.i.position + gravityNormal * 128.0f; - gameLocal.clip.Translation( tr, current.i.position, down, clipModel, current.i.orientation, clipMask, self ); + gameLocal.clip.Translation(tr, current.i.position, down, clipModel, current.i.orientation, clipMask, self); current.i.position = tr.endpos; - clipModel->Link( gameLocal.clip, self, clipModel->GetId(), tr.endpos, current.i.orientation ); + clipModel->Link(gameLocal.clip, self, clipModel->GetId(), tr.endpos, current.i.orientation); // if on the floor already - if ( tr.fraction == 0.0f ) { + if (tr.fraction == 0.0f) { // test if we are really at rest EvaluateContacts(); - if ( !TestIfAtRest() ) { - gameLocal.DWarning( "rigid body not at rest for entity '%s' type '%s' at (%s)", - self->name.c_str(), self->GetType()->classname, current.i.position.ToString(0) ); + if (!TestIfAtRest()) { + gameLocal.DWarning("rigid body not at rest for entity '%s' type '%s' at (%s)", + self->name.c_str(), self->GetType()->classname, current.i.position.ToString(0)); } Rest(); dropToFloor = false; - } else if ( IsOutsideWorld() ) { - gameLocal.Warning( "rigid body outside world bounds for entity '%s' type '%s' at (%s)", - self->name.c_str(), self->GetType()->classname, current.i.position.ToString(0) ); + } + else if (IsOutsideWorld()) { + gameLocal.Warning("rigid body outside world bounds for entity '%s' type '%s' at (%s)", + self->name.c_str(), self->GetType()->classname, current.i.position.ToString(0)); Rest(); dropToFloor = false; } @@ -403,27 +405,27 @@ void idPhysics_RigidBody::DropToFloorAndRest( void ) { idPhysics_RigidBody::DebugDraw ================ */ -void idPhysics_RigidBody::DebugDraw( void ) { +void idPhysics_RigidBody::DebugDraw(void) { - if ( rb_showBodies.GetBool() || ( rb_showActive.GetBool() && current.atRest < 0 ) ) { - collisionModelManager->DrawModel( clipModel->Handle(), clipModel->GetOrigin(), clipModel->GetAxis(), vec3_origin, 0.0f ); + if (rb_showBodies.GetBool() || (rb_showActive.GetBool() && current.atRest < 0)) { + collisionModelManager->DrawModel(clipModel->Handle(), clipModel->GetOrigin(), clipModel->GetAxis(), vec3_origin, 0.0f); } - if ( rb_showMass.GetBool() ) { - gameRenderWorld->DrawText( va( "\n%1.2f", mass ), current.i.position, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 ); + if (rb_showMass.GetBool()) { + gameRenderWorld->DrawText(va("\n%1.2f", mass), current.i.position, 0.08f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1); } - if ( rb_showInertia.GetBool() ) { - idMat3 &I = inertiaTensor; - gameRenderWorld->DrawText( va( "\n\n\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )", - I[0].x, I[0].y, I[0].z, - I[1].x, I[1].y, I[1].z, - I[2].x, I[2].y, I[2].z ), - current.i.position, 0.05f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1 ); + if (rb_showInertia.GetBool()) { + idMat3& I = inertiaTensor; + gameRenderWorld->DrawText(va("\n\n\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )\n( %.1f %.1f %.1f )", + I[0].x, I[0].y, I[0].z, + I[1].x, I[1].y, I[1].z, + I[2].x, I[2].y, I[2].z), + current.i.position, 0.05f, colorCyan, gameLocal.GetLocalPlayer()->viewAngles.ToMat3(), 1); } - if ( rb_showVelocity.GetBool() ) { - DrawVelocity( clipModel->GetId(), 0.1f, 4.0f ); + if (rb_showVelocity.GetBool()) { + DrawVelocity(clipModel->GetId(), 0.1f, 4.0f); } } @@ -432,15 +434,15 @@ void idPhysics_RigidBody::DebugDraw( void ) { idPhysics_RigidBody::idPhysics_RigidBody ================ */ -idPhysics_RigidBody::idPhysics_RigidBody( void ) { +idPhysics_RigidBody::idPhysics_RigidBody(void) { // set default rigid body properties - SetClipMask( MASK_SOLID ); - SetBouncyness( 0.6f ); - SetFriction( 0.6f, 0.6f, 0.0f ); + SetClipMask(MASK_SOLID); + SetBouncyness(0.6f); + SetFriction(0.6f, 0.6f, 0.0f); clipModel = NULL; - memset( ¤t, 0, sizeof( current ) ); + memset(¤t, 0, sizeof(current)); current.atRest = -1; current.lastTimeStep = USERCMD_MSEC; @@ -460,7 +462,7 @@ idPhysics_RigidBody::idPhysics_RigidBody( void ) { inverseInertiaTensor.Identity(); // use the least expensive euler integrator - integrator = new idODE_Euler( sizeof(rigidBodyIState_t) / sizeof(float), RigidBodyDerivatives, this ); + integrator = new idODE_Euler(sizeof(rigidBodyIState_t) / sizeof(float), RigidBodyDerivatives, this); dropToFloor = false; noImpact = false; @@ -479,8 +481,8 @@ idPhysics_RigidBody::idPhysics_RigidBody( void ) { idPhysics_RigidBody::~idPhysics_RigidBody ================ */ -idPhysics_RigidBody::~idPhysics_RigidBody( void ) { - if ( clipModel ) { +idPhysics_RigidBody::~idPhysics_RigidBody(void) { + if (clipModel) { delete clipModel; clipModel = NULL; } @@ -492,19 +494,19 @@ idPhysics_RigidBody::~idPhysics_RigidBody( void ) { idPhysics_RigidBody_SavePState ================ */ -void idPhysics_RigidBody_SavePState( idSaveGame *savefile, const rigidBodyPState_t &state ) { - savefile->WriteInt( state.atRest ); - savefile->WriteFloat( state.lastTimeStep ); - savefile->WriteVec3( state.localOrigin ); - savefile->WriteMat3( state.localAxis ); - savefile->WriteVec6( state.pushVelocity ); - savefile->WriteVec3( state.externalForce ); - savefile->WriteVec3( state.externalTorque ); +void idPhysics_RigidBody_SavePState(idSaveGame* savefile, const rigidBodyPState_t& state) { + savefile->WriteInt(state.atRest); + savefile->WriteFloat(state.lastTimeStep); + savefile->WriteVec3(state.localOrigin); + savefile->WriteMat3(state.localAxis); + savefile->WriteVec6(state.pushVelocity); + savefile->WriteVec3(state.externalForce); + savefile->WriteVec3(state.externalTorque); - savefile->WriteVec3( state.i.position ); - savefile->WriteMat3( state.i.orientation ); - savefile->WriteVec3( state.i.linearMomentum ); - savefile->WriteVec3( state.i.angularMomentum ); + savefile->WriteVec3(state.i.position); + savefile->WriteMat3(state.i.orientation); + savefile->WriteVec3(state.i.linearMomentum); + savefile->WriteVec3(state.i.angularMomentum); } /* @@ -512,19 +514,30 @@ void idPhysics_RigidBody_SavePState( idSaveGame *savefile, const rigidBodyPState idPhysics_RigidBody_RestorePState ================ */ -void idPhysics_RigidBody_RestorePState( idRestoreGame *savefile, rigidBodyPState_t &state ) { - savefile->ReadInt( state.atRest ); - savefile->ReadFloat( state.lastTimeStep ); - savefile->ReadVec3( state.localOrigin ); - savefile->ReadMat3( state.localAxis ); - savefile->ReadVec6( state.pushVelocity ); - savefile->ReadVec3( state.externalForce ); - savefile->ReadVec3( state.externalTorque ); +void idPhysics_RigidBody_RestorePState(idRestoreGame* savefile, rigidBodyPState_t& state) { + savefile->ReadInt(state.atRest); + savefile->ReadFloat(state.lastTimeStep); + savefile->ReadVec3(state.localOrigin); + savefile->ReadMat3(state.localAxis); + savefile->ReadVec6(state.pushVelocity); + savefile->ReadVec3(state.externalForce); + savefile->ReadVec3(state.externalTorque); - savefile->ReadVec3( state.i.position ); - savefile->ReadMat3( state.i.orientation ); - savefile->ReadVec3( state.i.linearMomentum ); - savefile->ReadVec3( state.i.angularMomentum ); + savefile->ReadVec3(state.i.position); + savefile->ReadMat3(state.i.orientation); + savefile->ReadVec3(state.i.linearMomentum); + savefile->ReadVec3(state.i.angularMomentum); + + state.localOrigin.FixDenormals(); + state.localAxis.FixDenormals(); + state.pushVelocity.SubVec3(0).FixDenormals(); + state.pushVelocity.SubVec3(1).FixDenormals(); + state.externalForce.FixDenormals(); + state.externalTorque.FixDenormals(); + state.i.position.FixDenormals(); + state.i.orientation.FixDenormals(); + state.i.linearMomentum.FixDenormals(); + state.i.angularMomentum.FixDenormals(); } /* @@ -532,30 +545,30 @@ void idPhysics_RigidBody_RestorePState( idRestoreGame *savefile, rigidBodyPState idPhysics_RigidBody::Save ================ */ -void idPhysics_RigidBody::Save( idSaveGame *savefile ) const { +void idPhysics_RigidBody::Save(idSaveGame* savefile) const { - idPhysics_RigidBody_SavePState( savefile, current ); - idPhysics_RigidBody_SavePState( savefile, saved ); + idPhysics_RigidBody_SavePState(savefile, current); + idPhysics_RigidBody_SavePState(savefile, saved); - savefile->WriteFloat( linearFriction ); - savefile->WriteFloat( angularFriction ); - savefile->WriteFloat( contactFriction ); - savefile->WriteFloat( bouncyness ); - savefile->WriteClipModel( clipModel ); + savefile->WriteFloat(linearFriction); + savefile->WriteFloat(angularFriction); + savefile->WriteFloat(contactFriction); + savefile->WriteFloat(bouncyness); + savefile->WriteClipModel(clipModel); - savefile->WriteFloat( mass ); - savefile->WriteFloat( inverseMass ); - savefile->WriteVec3( centerOfMass ); - savefile->WriteMat3( inertiaTensor ); - savefile->WriteMat3( inverseInertiaTensor ); + savefile->WriteFloat(mass); + savefile->WriteFloat(inverseMass); + savefile->WriteVec3(centerOfMass); + savefile->WriteMat3(inertiaTensor); + savefile->WriteMat3(inverseInertiaTensor); - savefile->WriteBool( dropToFloor ); - savefile->WriteBool( testSolid ); - savefile->WriteBool( noImpact ); - savefile->WriteBool( noContact ); + savefile->WriteBool(dropToFloor); + savefile->WriteBool(testSolid); + savefile->WriteBool(noImpact); + savefile->WriteBool(noContact); - savefile->WriteBool( hasMaster ); - savefile->WriteBool( isOrientated ); + savefile->WriteBool(hasMaster); + savefile->WriteBool(isOrientated); } /* @@ -563,30 +576,30 @@ void idPhysics_RigidBody::Save( idSaveGame *savefile ) const { idPhysics_RigidBody::Restore ================ */ -void idPhysics_RigidBody::Restore( idRestoreGame *savefile ) { +void idPhysics_RigidBody::Restore(idRestoreGame* savefile) { - idPhysics_RigidBody_RestorePState( savefile, current ); - idPhysics_RigidBody_RestorePState( savefile, saved ); + idPhysics_RigidBody_RestorePState(savefile, current); + idPhysics_RigidBody_RestorePState(savefile, saved); - savefile->ReadFloat( linearFriction ); - savefile->ReadFloat( angularFriction ); - savefile->ReadFloat( contactFriction ); - savefile->ReadFloat( bouncyness ); - savefile->ReadClipModel( clipModel ); + savefile->ReadFloat(linearFriction); + savefile->ReadFloat(angularFriction); + savefile->ReadFloat(contactFriction); + savefile->ReadFloat(bouncyness); + savefile->ReadClipModel(clipModel); - savefile->ReadFloat( mass ); - savefile->ReadFloat( inverseMass ); - savefile->ReadVec3( centerOfMass ); - savefile->ReadMat3( inertiaTensor ); - savefile->ReadMat3( inverseInertiaTensor ); + savefile->ReadFloat(mass); + savefile->ReadFloat(inverseMass); + savefile->ReadVec3(centerOfMass); + savefile->ReadMat3(inertiaTensor); + savefile->ReadMat3(inverseInertiaTensor); - savefile->ReadBool( dropToFloor ); - savefile->ReadBool( testSolid ); - savefile->ReadBool( noImpact ); - savefile->ReadBool( noContact ); + savefile->ReadBool(dropToFloor); + savefile->ReadBool(testSolid); + savefile->ReadBool(noImpact); + savefile->ReadBool(noContact); - savefile->ReadBool( hasMaster ); - savefile->ReadBool( isOrientated ); + savefile->ReadBool(hasMaster); + savefile->ReadBool(isOrientated); } /* @@ -596,51 +609,51 @@ idPhysics_RigidBody::SetClipModel */ #define MAX_INERTIA_SCALE 10.0f -void idPhysics_RigidBody::SetClipModel( idClipModel *model, const float density, int id, bool freeOld ) { +void idPhysics_RigidBody::SetClipModel(idClipModel* model, const float density, int id, bool freeOld) { int minIndex; idMat3 inertiaScale; - assert( self ); - assert( model ); // we need a clip model - assert( model->IsTraceModel() ); // and it should be a trace model - assert( density > 0.0f ); // density should be valid + assert(self); + assert(model); // we need a clip model + assert(model->IsTraceModel()); // and it should be a trace model + assert(density > 0.0f); // density should be valid - if ( clipModel && clipModel != model && freeOld ) { + if (clipModel && clipModel != model && freeOld) { delete clipModel; } clipModel = model; - clipModel->Link( gameLocal.clip, self, 0, current.i.position, current.i.orientation ); + clipModel->Link(gameLocal.clip, self, 0, current.i.position, current.i.orientation); // get mass properties from the trace model - clipModel->GetMassProperties( density, mass, centerOfMass, inertiaTensor ); + clipModel->GetMassProperties(density, mass, centerOfMass, inertiaTensor); // check whether or not the clip model has valid mass properties - if ( mass <= 0.0f || FLOAT_IS_NAN( mass ) ) { - gameLocal.Warning( "idPhysics_RigidBody::SetClipModel: invalid mass for entity '%s' type '%s'", - self->name.c_str(), self->GetType()->classname ); + if (mass < idMath::FLT_EPSILON || FLOAT_IS_NAN(mass)) { + gameLocal.Warning("idPhysics_RigidBody::SetClipModel: invalid mass for entity '%s' type '%s'", + self->name.c_str(), self->GetType()->classname); mass = 1.0f; centerOfMass.Zero(); inertiaTensor.Identity(); } // check whether or not the inertia tensor is balanced - minIndex = Min3Index( inertiaTensor[0][0], inertiaTensor[1][1], inertiaTensor[2][2] ); + minIndex = Min3Index(inertiaTensor[0][0], inertiaTensor[1][1], inertiaTensor[2][2]); inertiaScale.Identity(); inertiaScale[0][0] = inertiaTensor[0][0] / inertiaTensor[minIndex][minIndex]; inertiaScale[1][1] = inertiaTensor[1][1] / inertiaTensor[minIndex][minIndex]; inertiaScale[2][2] = inertiaTensor[2][2] / inertiaTensor[minIndex][minIndex]; - if ( inertiaScale[0][0] > MAX_INERTIA_SCALE || inertiaScale[1][1] > MAX_INERTIA_SCALE || inertiaScale[2][2] > MAX_INERTIA_SCALE ) { - gameLocal.DWarning( "idPhysics_RigidBody::SetClipModel: unbalanced inertia tensor for entity '%s' type '%s'", - self->name.c_str(), self->GetType()->classname ); + if (inertiaScale[0][0] > MAX_INERTIA_SCALE || inertiaScale[1][1] > MAX_INERTIA_SCALE || inertiaScale[2][2] > MAX_INERTIA_SCALE) { + gameLocal.DWarning("idPhysics_RigidBody::SetClipModel: unbalanced inertia tensor for entity '%s' type '%s'", + self->name.c_str(), self->GetType()->classname); float min = inertiaTensor[minIndex][minIndex] * MAX_INERTIA_SCALE; - inertiaScale[(minIndex+1)%3][(minIndex+1)%3] = min / inertiaTensor[(minIndex+1)%3][(minIndex+1)%3]; - inertiaScale[(minIndex+2)%3][(minIndex+2)%3] = min / inertiaTensor[(minIndex+2)%3][(minIndex+2)%3]; + inertiaScale[(minIndex + 1) % 3][(minIndex + 1) % 3] = min / inertiaTensor[(minIndex + 1) % 3][(minIndex + 1) % 3]; + inertiaScale[(minIndex + 2) % 3][(minIndex + 2) % 3] = min / inertiaTensor[(minIndex + 2) % 3][(minIndex + 2) % 3]; inertiaTensor *= inertiaScale; } inverseMass = 1.0f / mass; - inverseInertiaTensor = inertiaTensor.Inverse() * ( 1.0f / 6.0f ); + inverseInertiaTensor = inertiaTensor.Inverse() * (1.0f / 6.0f); current.i.linearMomentum.Zero(); current.i.angularMomentum.Zero(); @@ -651,7 +664,7 @@ void idPhysics_RigidBody::SetClipModel( idClipModel *model, const float density, idPhysics_RigidBody::GetClipModel ================ */ -idClipModel *idPhysics_RigidBody::GetClipModel( int id ) const { +idClipModel* idPhysics_RigidBody::GetClipModel(int id) const { return clipModel; } @@ -660,7 +673,7 @@ idClipModel *idPhysics_RigidBody::GetClipModel( int id ) const { idPhysics_RigidBody::GetNumClipModels ================ */ -int idPhysics_RigidBody::GetNumClipModels( void ) const { +int idPhysics_RigidBody::GetNumClipModels(void) const { return 1; } @@ -669,8 +682,8 @@ int idPhysics_RigidBody::GetNumClipModels( void ) const { idPhysics_RigidBody::SetMass ================ */ -void idPhysics_RigidBody::SetMass( float mass, int id ) { - assert( mass > 0.0f ); +void idPhysics_RigidBody::SetMass(float mass, int id) { + assert(mass > 0.0f); inertiaTensor *= mass / this->mass; inverseInertiaTensor = inertiaTensor.Inverse() * (1.0f / 6.0f); this->mass = mass; @@ -682,7 +695,7 @@ void idPhysics_RigidBody::SetMass( float mass, int id ) { idPhysics_RigidBody::GetMass ================ */ -float idPhysics_RigidBody::GetMass( int id ) const { +float idPhysics_RigidBody::GetMass(int id) const { return mass; } @@ -691,10 +704,11 @@ float idPhysics_RigidBody::GetMass( int id ) const { idPhysics_RigidBody::SetFriction ================ */ -void idPhysics_RigidBody::SetFriction( const float linear, const float angular, const float contact ) { - if ( linear < 0.0f || linear > 1.0f || - angular < 0.0f || angular > 1.0f || - contact < 0.0f || contact > 1.0f ) { +void idPhysics_RigidBody::SetFriction(const float linear, const float angular, const float contact) { + if (linear < 0.0f || linear > 1.0f || + angular < 0.0f || angular > 1.0f || + contact < 0.0f || contact > 1.0f) { + gameLocal.Warning("idPhysics_RigidBody::SetFriction: friction out of range, linear = %.1f, angular = %.1f, contact = %.1f", linear, angular, contact); return; } linearFriction = linear; @@ -707,8 +721,8 @@ void idPhysics_RigidBody::SetFriction( const float linear, const float angular, idPhysics_RigidBody::SetBouncyness ================ */ -void idPhysics_RigidBody::SetBouncyness( const float b ) { - if ( b < 0.0f || b > 1.0f ) { +void idPhysics_RigidBody::SetBouncyness(const float b) { + if (b < 0.0f || b > 1.0f) { return; } bouncyness = b; @@ -719,11 +733,11 @@ void idPhysics_RigidBody::SetBouncyness( const float b ) { idPhysics_RigidBody::Rest ================ */ -void idPhysics_RigidBody::Rest( void ) { +void idPhysics_RigidBody::Rest(void) { current.atRest = gameLocal.time; current.i.linearMomentum.Zero(); current.i.angularMomentum.Zero(); - self->BecomeInactive( TH_PHYSICS ); + self->BecomeInactive(TH_PHYSICS); } /* @@ -731,7 +745,7 @@ void idPhysics_RigidBody::Rest( void ) { idPhysics_RigidBody::DropToFloor ================ */ -void idPhysics_RigidBody::DropToFloor( void ) { +void idPhysics_RigidBody::DropToFloor(void) { dropToFloor = true; testSolid = true; } @@ -741,7 +755,7 @@ void idPhysics_RigidBody::DropToFloor( void ) { idPhysics_RigidBody::NoContact ================ */ -void idPhysics_RigidBody::NoContact( void ) { +void idPhysics_RigidBody::NoContact(void) { noContact = true; } @@ -750,9 +764,9 @@ void idPhysics_RigidBody::NoContact( void ) { idPhysics_RigidBody::Activate ================ */ -void idPhysics_RigidBody::Activate( void ) { +void idPhysics_RigidBody::Activate(void) { current.atRest = -1; - self->BecomeActive( TH_PHYSICS ); + self->BecomeActive(TH_PHYSICS); } /* @@ -762,7 +776,7 @@ idPhysics_RigidBody::PutToRest put to rest untill something collides with this physics object ================ */ -void idPhysics_RigidBody::PutToRest( void ) { +void idPhysics_RigidBody::PutToRest(void) { Rest(); } @@ -771,7 +785,7 @@ void idPhysics_RigidBody::PutToRest( void ) { idPhysics_RigidBody::EnableImpact ================ */ -void idPhysics_RigidBody::EnableImpact( void ) { +void idPhysics_RigidBody::EnableImpact(void) { noImpact = false; } @@ -780,7 +794,7 @@ void idPhysics_RigidBody::EnableImpact( void ) { idPhysics_RigidBody::DisableImpact ================ */ -void idPhysics_RigidBody::DisableImpact( void ) { +void idPhysics_RigidBody::DisableImpact(void) { noImpact = true; } @@ -789,8 +803,10 @@ void idPhysics_RigidBody::DisableImpact( void ) { idPhysics_RigidBody::SetContents ================ */ -void idPhysics_RigidBody::SetContents( int contents, int id ) { - clipModel->SetContents( contents ); +void idPhysics_RigidBody::SetContents(int contents, int id) { + if (clipModel) { + clipModel->SetContents(contents); + } } /* @@ -798,7 +814,7 @@ void idPhysics_RigidBody::SetContents( int contents, int id ) { idPhysics_RigidBody::GetContents ================ */ -int idPhysics_RigidBody::GetContents( int id ) const { +int idPhysics_RigidBody::GetContents(int id) const { return clipModel->GetContents(); } @@ -807,7 +823,7 @@ int idPhysics_RigidBody::GetContents( int id ) const { idPhysics_RigidBody::GetBounds ================ */ -const idBounds &idPhysics_RigidBody::GetBounds( int id ) const { +const idBounds& idPhysics_RigidBody::GetBounds(int id) const { return clipModel->GetBounds(); } @@ -816,7 +832,7 @@ const idBounds &idPhysics_RigidBody::GetBounds( int id ) const { idPhysics_RigidBody::GetAbsBounds ================ */ -const idBounds &idPhysics_RigidBody::GetAbsBounds( int id ) const { +const idBounds& idPhysics_RigidBody::GetAbsBounds(int id) const { return clipModel->GetAbsBounds(); } @@ -829,48 +845,56 @@ idPhysics_RigidBody::Evaluate the remaining time after the collision is ignored. ================ */ -bool idPhysics_RigidBody::Evaluate( int timeStepMSec, int endTimeMSec ) { +bool idPhysics_RigidBody::Evaluate(int timeStepMSec, int endTimeMSec) { rigidBodyPState_t next; idAngles angles; trace_t collision; idVec3 impulse; - idEntity *ent; + idEntity* ent; idVec3 oldOrigin, masterOrigin; idMat3 oldAxis, masterAxis; float timeStep; bool collided, cameToRest = false; - timeStep = MS2SEC( timeStepMSec ); + current.i.angularMomentum.FixDenormals(); + current.i.linearMomentum.FixDenormals(); + current.i.orientation.FixDenormals(); + current.i.position.FixDenormals(); + + timeStep = MS2SEC(timeStepMSec); current.lastTimeStep = timeStep; - if ( hasMaster ) { + if (hasMaster) { + if (timeStepMSec <= 0) { + return true; + } oldOrigin = current.i.position; oldAxis = current.i.orientation; - self->GetMasterPosition( masterOrigin, masterAxis ); + self->GetMasterPosition(masterOrigin, masterAxis); current.i.position = masterOrigin + current.localOrigin * masterAxis; - if ( isOrientated ) { + if (isOrientated) { current.i.orientation = current.localAxis * masterAxis; } else { current.i.orientation = current.localAxis; } - clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation ); - current.i.linearMomentum = mass * ( ( current.i.position - oldOrigin ) / timeStep ); - current.i.angularMomentum = inertiaTensor * ( ( current.i.orientation * oldAxis.Transpose() ).ToAngularVelocity() / timeStep ); + clipModel->Link(gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation); + current.i.linearMomentum = mass * ((current.i.position - oldOrigin) / timeStep); + current.i.angularMomentum = inertiaTensor * ((current.i.orientation * oldAxis.Transpose()).ToAngularVelocity() / timeStep); current.externalForce.Zero(); current.externalTorque.Zero(); - return ( current.i.position != oldOrigin || current.i.orientation != oldAxis ); + return (current.i.position != oldOrigin || current.i.orientation != oldAxis); } // if the body is at rest - if ( current.atRest >= 0 || timeStep <= 0.0f ) { + if (current.atRest >= 0 || timeStep <= 0.0f) { DebugDraw(); return false; } // if putting the body to rest - if ( dropToFloor ) { + if (dropToFloor) { DropToFloorAndRest(); current.externalForce.Zero(); current.externalTorque.Zero(); @@ -890,14 +914,14 @@ bool idPhysics_RigidBody::Evaluate( int timeStepMSec, int endTimeMSec ) { next = current; // calculate next position and orientation - Integrate( timeStep, next ); + Integrate(timeStep, next); #ifdef RB_TIMINGS timer_collision.Start(); #endif // check for collisions from the current to the next state - collided = CheckForCollisions( timeStep, next, collision ); + collided = CheckForCollisions(timeStep, next, collision); #ifdef RB_TIMINGS timer_collision.Stop(); @@ -906,19 +930,19 @@ bool idPhysics_RigidBody::Evaluate( int timeStepMSec, int endTimeMSec ) { // set the new state current = next; - if ( collided ) { + if (collided) { // apply collision impulse - if ( CollisionImpulse( collision, impulse ) ) { + if (CollisionImpulse(collision, impulse)) { current.atRest = gameLocal.time; } } // update the position of the clip model - clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation ); + clipModel->Link(gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation); DebugDraw(); - if ( !noContact ) { + if (!noContact) { #ifdef RB_TIMINGS timer_collision.Start(); @@ -931,26 +955,27 @@ bool idPhysics_RigidBody::Evaluate( int timeStepMSec, int endTimeMSec ) { #endif // check if the body has come to rest - if ( TestIfAtRest() ) { + if (TestIfAtRest()) { // put to rest Rest(); cameToRest = true; - } else { + } + else { // apply contact friction - ContactFriction( timeStep ); + ContactFriction(timeStep); } } - if ( current.atRest < 0 ) { + if (current.atRest < 0) { ActivateContactEntities(); } - if ( collided ) { + if (collided) { // if the rigid body didn't come to rest or the other entity is not at rest ent = gameLocal.entities[collision.c.entityNum]; - if ( ent && ( !cameToRest || !ent->IsAtRest() ) ) { + if (ent && (!cameToRest || !ent->IsAtRest())) { // apply impact to other entity - ent->ApplyImpulse( self, collision.c.id, collision.c.point, -impulse ); + ent->ApplyImpulse(self, collision.c.id, collision.c.point, -impulse); } } @@ -963,30 +988,30 @@ bool idPhysics_RigidBody::Evaluate( int timeStepMSec, int endTimeMSec ) { current.externalForce.Zero(); current.externalTorque.Zero(); - if ( IsOutsideWorld() ) { - gameLocal.Warning( "rigid body moved outside world bounds for entity '%s' type '%s' at (%s)", - self->name.c_str(), self->GetType()->classname, current.i.position.ToString(0) ); + if (IsOutsideWorld()) { + gameLocal.Warning("rigid body moved outside world bounds for entity '%s' type '%s' at (%s)", + self->name.c_str(), self->GetType()->classname, current.i.position.ToString(0)); Rest(); } #ifdef RB_TIMINGS timer_total.Stop(); - if ( rb_showTimings->integer == 1 ) { - gameLocal.Printf( "%12s: t %1.4f cd %1.4f\n", - self->name.c_str(), - timer_total.Milliseconds(), timer_collision.Milliseconds() ); + if (rb_showTimings->integer == 1) { + gameLocal.Printf("%12s: t %1.4f cd %1.4f\n", + self->name.c_str(), + timer_total.Milliseconds(), timer_collision.Milliseconds()); lastTimerReset = 0; } - else if ( rb_showTimings->integer == 2 ) { + else if (rb_showTimings->integer == 2) { numRigidBodies++; - if ( endTimeMSec > lastTimerReset ) { - gameLocal.Printf( "rb %d: t %1.4f cd %1.4f\n", - numRigidBodies, - timer_total.Milliseconds(), timer_collision.Milliseconds() ); + if (endTimeMSec > lastTimerReset) { + gameLocal.Printf("rb %d: t %1.4f cd %1.4f\n", + numRigidBodies, + timer_total.Milliseconds(), timer_collision.Milliseconds()); } } - if ( endTimeMSec > lastTimerReset ) { + if (endTimeMSec > lastTimerReset) { lastTimerReset = endTimeMSec; numRigidBodies = 0; timer_total.Clear(); @@ -1002,7 +1027,7 @@ bool idPhysics_RigidBody::Evaluate( int timeStepMSec, int endTimeMSec ) { idPhysics_RigidBody::UpdateTime ================ */ -void idPhysics_RigidBody::UpdateTime( int endTimeMSec ) { +void idPhysics_RigidBody::UpdateTime(int endTimeMSec) { } /* @@ -1010,7 +1035,7 @@ void idPhysics_RigidBody::UpdateTime( int endTimeMSec ) { idPhysics_RigidBody::GetTime ================ */ -int idPhysics_RigidBody::GetTime( void ) const { +int idPhysics_RigidBody::GetTime(void) const { return gameLocal.time; } @@ -1019,7 +1044,7 @@ int idPhysics_RigidBody::GetTime( void ) const { idPhysics_RigidBody::GetImpactInfo ================ */ -void idPhysics_RigidBody::GetImpactInfo( const int id, const idVec3 &point, impactInfo_t *info ) const { +void idPhysics_RigidBody::GetImpactInfo(const int id, const idVec3& point, impactInfo_t* info) const { idVec3 linearVelocity, angularVelocity; idMat3 inverseWorldInertiaTensor; @@ -1029,8 +1054,8 @@ void idPhysics_RigidBody::GetImpactInfo( const int id, const idVec3 &point, impa info->invMass = inverseMass; info->invInertiaTensor = inverseWorldInertiaTensor; - info->position = point - ( current.i.position + centerOfMass * current.i.orientation ); - info->velocity = linearVelocity + angularVelocity.Cross( info->position ); + info->position = point - (current.i.position + centerOfMass * current.i.orientation); + info->velocity = linearVelocity + angularVelocity.Cross(info->position); } /* @@ -1038,12 +1063,12 @@ void idPhysics_RigidBody::GetImpactInfo( const int id, const idVec3 &point, impa idPhysics_RigidBody::ApplyImpulse ================ */ -void idPhysics_RigidBody::ApplyImpulse( const int id, const idVec3 &point, const idVec3 &impulse ) { - if ( noImpact ) { +void idPhysics_RigidBody::ApplyImpulse(const int id, const idVec3& point, const idVec3& impulse) { + if (noImpact) { return; } current.i.linearMomentum += impulse; - current.i.angularMomentum += ( point - ( current.i.position + centerOfMass * current.i.orientation ) ).Cross( impulse ); + current.i.angularMomentum += (point - (current.i.position + centerOfMass * current.i.orientation)).Cross(impulse); Activate(); } @@ -1052,12 +1077,12 @@ void idPhysics_RigidBody::ApplyImpulse( const int id, const idVec3 &point, const idPhysics_RigidBody::AddForce ================ */ -void idPhysics_RigidBody::AddForce( const int id, const idVec3 &point, const idVec3 &force ) { - if ( noImpact ) { +void idPhysics_RigidBody::AddForce(const int id, const idVec3& point, const idVec3& force) { + if (noImpact) { return; } current.externalForce += force; - current.externalTorque += ( point - ( current.i.position + centerOfMass * current.i.orientation ) ).Cross( force ); + current.externalTorque += (point - (current.i.position + centerOfMass * current.i.orientation)).Cross(force); Activate(); } @@ -1066,7 +1091,7 @@ void idPhysics_RigidBody::AddForce( const int id, const idVec3 &point, const idV idPhysics_RigidBody::IsAtRest ================ */ -bool idPhysics_RigidBody::IsAtRest( void ) const { +bool idPhysics_RigidBody::IsAtRest(void) const { return current.atRest >= 0; } @@ -1075,7 +1100,7 @@ bool idPhysics_RigidBody::IsAtRest( void ) const { idPhysics_RigidBody::GetRestStartTime ================ */ -int idPhysics_RigidBody::GetRestStartTime( void ) const { +int idPhysics_RigidBody::GetRestStartTime(void) const { return current.atRest; } @@ -1084,8 +1109,8 @@ int idPhysics_RigidBody::GetRestStartTime( void ) const { idPhysics_RigidBody::IsPushable ================ */ -bool idPhysics_RigidBody::IsPushable( void ) const { - return ( !noImpact && !hasMaster ); +bool idPhysics_RigidBody::IsPushable(void) const { + return (!noImpact && !hasMaster); } /* @@ -1093,7 +1118,7 @@ bool idPhysics_RigidBody::IsPushable( void ) const { idPhysics_RigidBody::SaveState ================ */ -void idPhysics_RigidBody::SaveState( void ) { +void idPhysics_RigidBody::SaveState(void) { saved = current; } @@ -1102,10 +1127,10 @@ void idPhysics_RigidBody::SaveState( void ) { idPhysics_RigidBody::RestoreState ================ */ -void idPhysics_RigidBody::RestoreState( void ) { +void idPhysics_RigidBody::RestoreState(void) { current = saved; - clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation ); + clipModel->Link(gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation); EvaluateContacts(); } @@ -1115,20 +1140,20 @@ void idPhysics_RigidBody::RestoreState( void ) { idPhysics::SetOrigin ================ */ -void idPhysics_RigidBody::SetOrigin( const idVec3 &newOrigin, int id ) { +void idPhysics_RigidBody::SetOrigin(const idVec3& newOrigin, int id) { idVec3 masterOrigin; idMat3 masterAxis; current.localOrigin = newOrigin; - if ( hasMaster ) { - self->GetMasterPosition( masterOrigin, masterAxis ); + if (hasMaster) { + self->GetMasterPosition(masterOrigin, masterAxis); current.i.position = masterOrigin + newOrigin * masterAxis; } else { current.i.position = newOrigin; } - clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.i.position, clipModel->GetAxis() ); + clipModel->Link(gameLocal.clip, self, clipModel->GetId(), current.i.position, clipModel->GetAxis()); Activate(); } @@ -1138,20 +1163,20 @@ void idPhysics_RigidBody::SetOrigin( const idVec3 &newOrigin, int id ) { idPhysics::SetAxis ================ */ -void idPhysics_RigidBody::SetAxis( const idMat3 &newAxis, int id ) { +void idPhysics_RigidBody::SetAxis(const idMat3& newAxis, int id) { idVec3 masterOrigin; idMat3 masterAxis; current.localAxis = newAxis; - if ( hasMaster && isOrientated ) { - self->GetMasterPosition( masterOrigin, masterAxis ); + if (hasMaster && isOrientated) { + self->GetMasterPosition(masterOrigin, masterAxis); current.i.orientation = newAxis * masterAxis; } else { current.i.orientation = newAxis; } - clipModel->Link( gameLocal.clip, self, clipModel->GetId(), clipModel->GetOrigin(), current.i.orientation ); + clipModel->Link(gameLocal.clip, self, clipModel->GetId(), clipModel->GetOrigin(), current.i.orientation); Activate(); } @@ -1161,12 +1186,12 @@ void idPhysics_RigidBody::SetAxis( const idMat3 &newAxis, int id ) { idPhysics::Move ================ */ -void idPhysics_RigidBody::Translate( const idVec3 &translation, int id ) { +void idPhysics_RigidBody::Translate(const idVec3& translation, int id) { current.localOrigin += translation; current.i.position += translation; - clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.i.position, clipModel->GetAxis() ); + clipModel->Link(gameLocal.clip, self, clipModel->GetId(), current.i.position, clipModel->GetAxis()); Activate(); } @@ -1176,24 +1201,24 @@ void idPhysics_RigidBody::Translate( const idVec3 &translation, int id ) { idPhysics::Rotate ================ */ -void idPhysics_RigidBody::Rotate( const idRotation &rotation, int id ) { +void idPhysics_RigidBody::Rotate(const idRotation& rotation, int id) { idVec3 masterOrigin; idMat3 masterAxis; current.i.orientation *= rotation.ToMat3(); current.i.position *= rotation; - if ( hasMaster ) { - self->GetMasterPosition( masterOrigin, masterAxis ); + if (hasMaster) { + self->GetMasterPosition(masterOrigin, masterAxis); current.localAxis *= rotation.ToMat3(); - current.localOrigin = ( current.i.position - masterOrigin ) * masterAxis.Transpose(); + current.localOrigin = (current.i.position - masterOrigin) * masterAxis.Transpose(); } else { current.localAxis = current.i.orientation; current.localOrigin = current.i.position; } - clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation ); + clipModel->Link(gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation); Activate(); } @@ -1203,7 +1228,7 @@ void idPhysics_RigidBody::Rotate( const idRotation &rotation, int id ) { idPhysics_RigidBody::GetOrigin ================ */ -const idVec3 &idPhysics_RigidBody::GetOrigin( int id ) const { +const idVec3& idPhysics_RigidBody::GetOrigin(int id) const { return current.i.position; } @@ -1212,7 +1237,7 @@ const idVec3 &idPhysics_RigidBody::GetOrigin( int id ) const { idPhysics_RigidBody::GetAxis ================ */ -const idMat3 &idPhysics_RigidBody::GetAxis( int id ) const { +const idMat3& idPhysics_RigidBody::GetAxis(int id) const { return current.i.orientation; } @@ -1221,7 +1246,7 @@ const idMat3 &idPhysics_RigidBody::GetAxis( int id ) const { idPhysics_RigidBody::SetLinearVelocity ================ */ -void idPhysics_RigidBody::SetLinearVelocity( const idVec3 &newLinearVelocity, int id ) { +void idPhysics_RigidBody::SetLinearVelocity(const idVec3& newLinearVelocity, int id) { current.i.linearMomentum = newLinearVelocity * mass; Activate(); } @@ -1231,8 +1256,9 @@ void idPhysics_RigidBody::SetLinearVelocity( const idVec3 &newLinearVelocity, in idPhysics_RigidBody::SetAngularVelocity ================ */ -void idPhysics_RigidBody::SetAngularVelocity( const idVec3 &newAngularVelocity, int id ) { - current.i.angularMomentum = newAngularVelocity * inertiaTensor; +void idPhysics_RigidBody::SetAngularVelocity(const idVec3& newAngularVelocity, int id) { + idMat3 worldInertiaTensor = 6.0f * (current.i.orientation.Transpose() * inertiaTensor * current.i.orientation); + current.i.angularMomentum = worldInertiaTensor * newAngularVelocity; Activate(); } @@ -1241,7 +1267,7 @@ void idPhysics_RigidBody::SetAngularVelocity( const idVec3 &newAngularVelocity, idPhysics_RigidBody::GetLinearVelocity ================ */ -const idVec3 &idPhysics_RigidBody::GetLinearVelocity( int id ) const { +const idVec3& idPhysics_RigidBody::GetLinearVelocity(int id) const { static idVec3 curLinearVelocity; curLinearVelocity = current.i.linearMomentum * inverseMass; return curLinearVelocity; @@ -1252,7 +1278,7 @@ const idVec3 &idPhysics_RigidBody::GetLinearVelocity( int id ) const { idPhysics_RigidBody::GetAngularVelocity ================ */ -const idVec3 &idPhysics_RigidBody::GetAngularVelocity( int id ) const { +const idVec3& idPhysics_RigidBody::GetAngularVelocity(int id) const { static idVec3 curAngularVelocity; idMat3 inverseWorldInertiaTensor; @@ -1266,15 +1292,15 @@ const idVec3 &idPhysics_RigidBody::GetAngularVelocity( int id ) const { idPhysics_RigidBody::ClipTranslation ================ */ -void idPhysics_RigidBody::ClipTranslation( trace_t &results, const idVec3 &translation, const idClipModel *model ) const { - if ( model ) { - gameLocal.clip.TranslationModel( results, clipModel->GetOrigin(), clipModel->GetOrigin() + translation, - clipModel, clipModel->GetAxis(), clipMask, - model->Handle(), model->GetOrigin(), model->GetAxis() ); +void idPhysics_RigidBody::ClipTranslation(trace_t& results, const idVec3& translation, const idClipModel* model) const { + if (model) { + gameLocal.clip.TranslationModel(results, clipModel->GetOrigin(), clipModel->GetOrigin() + translation, + clipModel, clipModel->GetAxis(), clipMask, + model->Handle(), model->GetOrigin(), model->GetAxis()); } else { - gameLocal.clip.Translation( results, clipModel->GetOrigin(), clipModel->GetOrigin() + translation, - clipModel, clipModel->GetAxis(), clipMask, self ); + gameLocal.clip.Translation(results, clipModel->GetOrigin(), clipModel->GetOrigin() + translation, + clipModel, clipModel->GetAxis(), clipMask, self); } } @@ -1283,15 +1309,15 @@ void idPhysics_RigidBody::ClipTranslation( trace_t &results, const idVec3 &trans idPhysics_RigidBody::ClipRotation ================ */ -void idPhysics_RigidBody::ClipRotation( trace_t &results, const idRotation &rotation, const idClipModel *model ) const { - if ( model ) { - gameLocal.clip.RotationModel( results, clipModel->GetOrigin(), rotation, - clipModel, clipModel->GetAxis(), clipMask, - model->Handle(), model->GetOrigin(), model->GetAxis() ); +void idPhysics_RigidBody::ClipRotation(trace_t& results, const idRotation& rotation, const idClipModel* model) const { + if (model) { + gameLocal.clip.RotationModel(results, clipModel->GetOrigin(), rotation, + clipModel, clipModel->GetAxis(), clipMask, + model->Handle(), model->GetOrigin(), model->GetAxis()); } else { - gameLocal.clip.Rotation( results, clipModel->GetOrigin(), rotation, - clipModel, clipModel->GetAxis(), clipMask, self ); + gameLocal.clip.Rotation(results, clipModel->GetOrigin(), rotation, + clipModel, clipModel->GetAxis(), clipMask, self); } } @@ -1300,13 +1326,13 @@ void idPhysics_RigidBody::ClipRotation( trace_t &results, const idRotation &rota idPhysics_RigidBody::ClipContents ================ */ -int idPhysics_RigidBody::ClipContents( const idClipModel *model ) const { - if ( model ) { - return gameLocal.clip.ContentsModel( clipModel->GetOrigin(), clipModel, clipModel->GetAxis(), -1, - model->Handle(), model->GetOrigin(), model->GetAxis() ); +int idPhysics_RigidBody::ClipContents(const idClipModel* model) const { + if (model) { + return gameLocal.clip.ContentsModel(clipModel->GetOrigin(), clipModel, clipModel->GetAxis(), -1, + model->Handle(), model->GetOrigin(), model->GetAxis()); } else { - return gameLocal.clip.Contents( clipModel->GetOrigin(), clipModel, clipModel->GetAxis(), -1, NULL ); + return gameLocal.clip.Contents(clipModel->GetOrigin(), clipModel, clipModel->GetAxis(), -1, NULL); } } @@ -1315,7 +1341,7 @@ int idPhysics_RigidBody::ClipContents( const idClipModel *model ) const { idPhysics_RigidBody::DisableClip ================ */ -void idPhysics_RigidBody::DisableClip( void ) { +void idPhysics_RigidBody::DisableClip(void) { clipModel->Disable(); } @@ -1324,7 +1350,7 @@ void idPhysics_RigidBody::DisableClip( void ) { idPhysics_RigidBody::EnableClip ================ */ -void idPhysics_RigidBody::EnableClip( void ) { +void idPhysics_RigidBody::EnableClip(void) { clipModel->Enable(); } @@ -1333,7 +1359,7 @@ void idPhysics_RigidBody::EnableClip( void ) { idPhysics_RigidBody::UnlinkClip ================ */ -void idPhysics_RigidBody::UnlinkClip( void ) { +void idPhysics_RigidBody::UnlinkClip(void) { clipModel->Unlink(); } @@ -1342,8 +1368,8 @@ void idPhysics_RigidBody::UnlinkClip( void ) { idPhysics_RigidBody::LinkClip ================ */ -void idPhysics_RigidBody::LinkClip( void ) { - clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation ); +void idPhysics_RigidBody::LinkClip(void) { + clipModel->Link(gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation); } /* @@ -1351,25 +1377,25 @@ void idPhysics_RigidBody::LinkClip( void ) { idPhysics_RigidBody::EvaluateContacts ================ */ -bool idPhysics_RigidBody::EvaluateContacts( void ) { +bool idPhysics_RigidBody::EvaluateContacts(void) { idVec6 dir; int num; ClearContacts(); - contacts.SetNum( 10, false ); + contacts.SetNum(10, false); dir.SubVec3(0) = current.i.linearMomentum + current.lastTimeStep * gravityVector * mass; dir.SubVec3(1) = current.i.angularMomentum; dir.SubVec3(0).Normalize(); dir.SubVec3(1).Normalize(); - num = gameLocal.clip.Contacts( &contacts[0], 10, clipModel->GetOrigin(), - dir, CONTACT_EPSILON, clipModel, clipModel->GetAxis(), clipMask, self ); - contacts.SetNum( num, false ); + num = gameLocal.clip.Contacts(&contacts[0], 10, clipModel->GetOrigin(), + dir, CONTACT_EPSILON, clipModel, clipModel->GetAxis(), clipMask, self); + contacts.SetNum(num, false); AddContactEntitiesForContacts(); - return ( contacts.Num() != 0 ); + return (contacts.Num() != 0); } /* @@ -1377,14 +1403,14 @@ bool idPhysics_RigidBody::EvaluateContacts( void ) { idPhysics_RigidBody::SetPushed ================ */ -void idPhysics_RigidBody::SetPushed( int deltaTime ) { +void idPhysics_RigidBody::SetPushed(int deltaTime) { idRotation rotation; - rotation = ( saved.i.orientation * current.i.orientation ).ToRotation(); + rotation = (saved.i.orientation * current.i.orientation).ToRotation(); // velocity with which the af is pushed - current.pushVelocity.SubVec3(0) += ( current.i.position - saved.i.position ) / ( deltaTime * idMath::M_MS2SEC ); - current.pushVelocity.SubVec3(1) += rotation.GetVec() * -DEG2RAD( rotation.GetAngle() ) / ( deltaTime * idMath::M_MS2SEC ); + current.pushVelocity.SubVec3(0) += (current.i.position - saved.i.position) / (deltaTime * idMath::M_MS2SEC); + current.pushVelocity.SubVec3(1) += rotation.GetVec() * -DEG2RAD(rotation.GetAngle()) / (deltaTime * idMath::M_MS2SEC); } /* @@ -1392,7 +1418,7 @@ void idPhysics_RigidBody::SetPushed( int deltaTime ) { idPhysics_RigidBody::GetPushedLinearVelocity ================ */ -const idVec3 &idPhysics_RigidBody::GetPushedLinearVelocity( const int id ) const { +const idVec3& idPhysics_RigidBody::GetPushedLinearVelocity(const int id) const { return current.pushVelocity.SubVec3(0); } @@ -1401,7 +1427,7 @@ const idVec3 &idPhysics_RigidBody::GetPushedLinearVelocity( const int id ) const idPhysics_RigidBody::GetPushedAngularVelocity ================ */ -const idVec3 &idPhysics_RigidBody::GetPushedAngularVelocity( const int id ) const { +const idVec3& idPhysics_RigidBody::GetPushedAngularVelocity(const int id) const { return current.pushVelocity.SubVec3(1); } @@ -1410,16 +1436,16 @@ const idVec3 &idPhysics_RigidBody::GetPushedAngularVelocity( const int id ) cons idPhysics_RigidBody::SetMaster ================ */ -void idPhysics_RigidBody::SetMaster( idEntity *master, const bool orientated ) { +void idPhysics_RigidBody::SetMaster(idEntity* master, const bool orientated) { idVec3 masterOrigin; idMat3 masterAxis; - if ( master ) { - if ( !hasMaster ) { + if (master) { + if (!hasMaster) { // transform from world space to master space - self->GetMasterPosition( masterOrigin, masterAxis ); - current.localOrigin = ( current.i.position - masterOrigin ) * masterAxis.Transpose(); - if ( orientated ) { + self->GetMasterPosition(masterOrigin, masterAxis); + current.localOrigin = (current.i.position - masterOrigin) * masterAxis.Transpose(); + if (orientated) { current.localAxis = current.i.orientation * masterAxis.Transpose(); } else { @@ -1431,65 +1457,68 @@ void idPhysics_RigidBody::SetMaster( idEntity *master, const bool orientated ) { } } else { - if ( hasMaster ) { + current.localOrigin = vec3_origin; + current.localAxis = mat3_identity; + + if (hasMaster) { hasMaster = false; Activate(); } } } -const float RB_VELOCITY_MAX = 16000; -const int RB_VELOCITY_TOTAL_BITS = 16; -const int RB_VELOCITY_EXPONENT_BITS = idMath::BitsForInteger( idMath::BitsForFloat( RB_VELOCITY_MAX ) ) + 1; -const int RB_VELOCITY_MANTISSA_BITS = RB_VELOCITY_TOTAL_BITS - 1 - RB_VELOCITY_EXPONENT_BITS; -const float RB_MOMENTUM_MAX = 1e20f; -const int RB_MOMENTUM_TOTAL_BITS = 16; -const int RB_MOMENTUM_EXPONENT_BITS = idMath::BitsForInteger( idMath::BitsForFloat( RB_MOMENTUM_MAX ) ) + 1; -const int RB_MOMENTUM_MANTISSA_BITS = RB_MOMENTUM_TOTAL_BITS - 1 - RB_MOMENTUM_EXPONENT_BITS; -const float RB_FORCE_MAX = 1e20f; -const int RB_FORCE_TOTAL_BITS = 16; -const int RB_FORCE_EXPONENT_BITS = idMath::BitsForInteger( idMath::BitsForFloat( RB_FORCE_MAX ) ) + 1; -const int RB_FORCE_MANTISSA_BITS = RB_FORCE_TOTAL_BITS - 1 - RB_FORCE_EXPONENT_BITS; +const float RB_VELOCITY_MAX = 16000; +const int RB_VELOCITY_TOTAL_BITS = 16; +const int RB_VELOCITY_EXPONENT_BITS = idMath::BitsForInteger(idMath::BitsForFloat(RB_VELOCITY_MAX)) + 1; +const int RB_VELOCITY_MANTISSA_BITS = RB_VELOCITY_TOTAL_BITS - 1 - RB_VELOCITY_EXPONENT_BITS; +const float RB_MOMENTUM_MAX = 1e20f; +const int RB_MOMENTUM_TOTAL_BITS = 16; +const int RB_MOMENTUM_EXPONENT_BITS = idMath::BitsForInteger(idMath::BitsForFloat(RB_MOMENTUM_MAX)) + 1; +const int RB_MOMENTUM_MANTISSA_BITS = RB_MOMENTUM_TOTAL_BITS - 1 - RB_MOMENTUM_EXPONENT_BITS; +const float RB_FORCE_MAX = 1e20f; +const int RB_FORCE_TOTAL_BITS = 16; +const int RB_FORCE_EXPONENT_BITS = idMath::BitsForInteger(idMath::BitsForFloat(RB_FORCE_MAX)) + 1; +const int RB_FORCE_MANTISSA_BITS = RB_FORCE_TOTAL_BITS - 1 - RB_FORCE_EXPONENT_BITS; /* ================ idPhysics_RigidBody::WriteToSnapshot ================ */ -void idPhysics_RigidBody::WriteToSnapshot( idBitMsgDelta &msg ) const { +void idPhysics_RigidBody::WriteToSnapshot(idBitMsgDelta& msg) const { idCQuat quat, localQuat; quat = current.i.orientation.ToCQuat(); localQuat = current.localAxis.ToCQuat(); - msg.WriteLong( current.atRest ); - msg.WriteFloat( current.i.position[0] ); - msg.WriteFloat( current.i.position[1] ); - msg.WriteFloat( current.i.position[2] ); - msg.WriteFloat( quat.x ); - msg.WriteFloat( quat.y ); - msg.WriteFloat( quat.z ); - msg.WriteFloat( current.i.linearMomentum[0], RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS ); - msg.WriteFloat( current.i.linearMomentum[1], RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS ); - msg.WriteFloat( current.i.linearMomentum[2], RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS ); - msg.WriteFloat( current.i.angularMomentum[0], RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS ); - msg.WriteFloat( current.i.angularMomentum[1], RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS ); - msg.WriteFloat( current.i.angularMomentum[2], RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS ); - msg.WriteDeltaFloat( current.i.position[0], current.localOrigin[0] ); - msg.WriteDeltaFloat( current.i.position[1], current.localOrigin[1] ); - msg.WriteDeltaFloat( current.i.position[2], current.localOrigin[2] ); - msg.WriteDeltaFloat( quat.x, localQuat.x ); - msg.WriteDeltaFloat( quat.y, localQuat.y ); - msg.WriteDeltaFloat( quat.z, localQuat.z ); - msg.WriteDeltaFloat( 0.0f, current.pushVelocity[0], RB_VELOCITY_EXPONENT_BITS, RB_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, current.pushVelocity[1], RB_VELOCITY_EXPONENT_BITS, RB_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, current.pushVelocity[2], RB_VELOCITY_EXPONENT_BITS, RB_VELOCITY_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, current.externalForce[0], RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, current.externalForce[1], RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, current.externalForce[2], RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, current.externalTorque[0], RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, current.externalTorque[1], RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS ); - msg.WriteDeltaFloat( 0.0f, current.externalTorque[2], RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS ); + msg.WriteLong(current.atRest); + msg.WriteFloat(current.i.position[0]); + msg.WriteFloat(current.i.position[1]); + msg.WriteFloat(current.i.position[2]); + msg.WriteFloat(quat.x); + msg.WriteFloat(quat.y); + msg.WriteFloat(quat.z); + msg.WriteFloat(current.i.linearMomentum[0], RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS); + msg.WriteFloat(current.i.linearMomentum[1], RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS); + msg.WriteFloat(current.i.linearMomentum[2], RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS); + msg.WriteFloat(current.i.angularMomentum[0], RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS); + msg.WriteFloat(current.i.angularMomentum[1], RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS); + msg.WriteFloat(current.i.angularMomentum[2], RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS); + msg.WriteDeltaFloat(current.i.position[0], current.localOrigin[0]); + msg.WriteDeltaFloat(current.i.position[1], current.localOrigin[1]); + msg.WriteDeltaFloat(current.i.position[2], current.localOrigin[2]); + msg.WriteDeltaFloat(quat.x, localQuat.x); + msg.WriteDeltaFloat(quat.y, localQuat.y); + msg.WriteDeltaFloat(quat.z, localQuat.z); + msg.WriteDeltaFloat(0.0f, current.pushVelocity[0], RB_VELOCITY_EXPONENT_BITS, RB_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, current.pushVelocity[1], RB_VELOCITY_EXPONENT_BITS, RB_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, current.pushVelocity[2], RB_VELOCITY_EXPONENT_BITS, RB_VELOCITY_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, current.externalForce[0], RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, current.externalForce[1], RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, current.externalForce[2], RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, current.externalTorque[0], RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, current.externalTorque[1], RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS); + msg.WriteDeltaFloat(0.0f, current.externalTorque[2], RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS); } /* @@ -1497,7 +1526,7 @@ void idPhysics_RigidBody::WriteToSnapshot( idBitMsgDelta &msg ) const { idPhysics_RigidBody::ReadFromSnapshot ================ */ -void idPhysics_RigidBody::ReadFromSnapshot( const idBitMsgDelta &msg ) { +void idPhysics_RigidBody::ReadFromSnapshot(const idBitMsgDelta& msg) { idCQuat quat, localQuat; current.atRest = msg.ReadLong(); @@ -1507,32 +1536,43 @@ void idPhysics_RigidBody::ReadFromSnapshot( const idBitMsgDelta &msg ) { quat.x = msg.ReadFloat(); quat.y = msg.ReadFloat(); quat.z = msg.ReadFloat(); - current.i.linearMomentum[0] = msg.ReadFloat( RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS ); - current.i.linearMomentum[1] = msg.ReadFloat( RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS ); - current.i.linearMomentum[2] = msg.ReadFloat( RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS ); - current.i.angularMomentum[0] = msg.ReadFloat( RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS ); - current.i.angularMomentum[1] = msg.ReadFloat( RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS ); - current.i.angularMomentum[2] = msg.ReadFloat( RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS ); - current.localOrigin[0] = msg.ReadDeltaFloat( current.i.position[0] ); - current.localOrigin[1] = msg.ReadDeltaFloat( current.i.position[1] ); - current.localOrigin[2] = msg.ReadDeltaFloat( current.i.position[2] ); - localQuat.x = msg.ReadDeltaFloat( quat.x ); - localQuat.y = msg.ReadDeltaFloat( quat.y ); - localQuat.z = msg.ReadDeltaFloat( quat.z ); - current.pushVelocity[0] = msg.ReadDeltaFloat( 0.0f, RB_VELOCITY_EXPONENT_BITS, RB_VELOCITY_MANTISSA_BITS ); - current.pushVelocity[1] = msg.ReadDeltaFloat( 0.0f, RB_VELOCITY_EXPONENT_BITS, RB_VELOCITY_MANTISSA_BITS ); - current.pushVelocity[2] = msg.ReadDeltaFloat( 0.0f, RB_VELOCITY_EXPONENT_BITS, RB_VELOCITY_MANTISSA_BITS ); - current.externalForce[0] = msg.ReadDeltaFloat( 0.0f, RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS ); - current.externalForce[1] = msg.ReadDeltaFloat( 0.0f, RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS ); - current.externalForce[2] = msg.ReadDeltaFloat( 0.0f, RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS ); - current.externalTorque[0] = msg.ReadDeltaFloat( 0.0f, RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS ); - current.externalTorque[1] = msg.ReadDeltaFloat( 0.0f, RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS ); - current.externalTorque[2] = msg.ReadDeltaFloat( 0.0f, RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS ); + current.i.linearMomentum[0] = msg.ReadFloat(RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS); + current.i.linearMomentum[1] = msg.ReadFloat(RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS); + current.i.linearMomentum[2] = msg.ReadFloat(RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS); + current.i.angularMomentum[0] = msg.ReadFloat(RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS); + current.i.angularMomentum[1] = msg.ReadFloat(RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS); + current.i.angularMomentum[2] = msg.ReadFloat(RB_MOMENTUM_EXPONENT_BITS, RB_MOMENTUM_MANTISSA_BITS); + current.localOrigin[0] = msg.ReadDeltaFloat(current.i.position[0]); + current.localOrigin[1] = msg.ReadDeltaFloat(current.i.position[1]); + current.localOrigin[2] = msg.ReadDeltaFloat(current.i.position[2]); + localQuat.x = msg.ReadDeltaFloat(quat.x); + localQuat.y = msg.ReadDeltaFloat(quat.y); + localQuat.z = msg.ReadDeltaFloat(quat.z); + current.pushVelocity[0] = msg.ReadDeltaFloat(0.0f, RB_VELOCITY_EXPONENT_BITS, RB_VELOCITY_MANTISSA_BITS); + current.pushVelocity[1] = msg.ReadDeltaFloat(0.0f, RB_VELOCITY_EXPONENT_BITS, RB_VELOCITY_MANTISSA_BITS); + current.pushVelocity[2] = msg.ReadDeltaFloat(0.0f, RB_VELOCITY_EXPONENT_BITS, RB_VELOCITY_MANTISSA_BITS); + current.externalForce[0] = msg.ReadDeltaFloat(0.0f, RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS); + current.externalForce[1] = msg.ReadDeltaFloat(0.0f, RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS); + current.externalForce[2] = msg.ReadDeltaFloat(0.0f, RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS); + current.externalTorque[0] = msg.ReadDeltaFloat(0.0f, RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS); + current.externalTorque[1] = msg.ReadDeltaFloat(0.0f, RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS); + current.externalTorque[2] = msg.ReadDeltaFloat(0.0f, RB_FORCE_EXPONENT_BITS, RB_FORCE_MANTISSA_BITS); current.i.orientation = quat.ToMat3(); current.localAxis = localQuat.ToMat3(); - if ( clipModel ) { - clipModel->Link( gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation ); + current.i.position.FixDenormals(); + current.i.orientation.FixDenormals(); + current.i.linearMomentum.FixDenormals(); + current.i.angularMomentum.FixDenormals(); + current.localOrigin.FixDenormals(); + current.localAxis.FixDenormals(); + current.pushVelocity.SubVec3(0).FixDenormals(); + current.pushVelocity.SubVec3(1).FixDenormals(); + current.externalForce.FixDenormals(); + current.externalTorque.FixDenormals(); + + if (clipModel) { + clipModel->Link(gameLocal.clip, self, clipModel->GetId(), current.i.position, current.i.orientation); } } diff --git a/neo/idlib/bv/Bounds.h b/neo/idlib/bv/Bounds.h index f164a327..fe2fdc01 100644 --- a/neo/idlib/bv/Bounds.h +++ b/neo/idlib/bv/Bounds.h @@ -62,6 +62,8 @@ public: void Clear( void ); // inside out bounds void Zero( void ); // single point at origin + idVec3 Size(void) const; + idVec3 GetCenter( void ) const; // returns center of bounds float GetRadius( void ) const; // returns the radius relative to the bounds origin float GetRadius( const idVec3 ¢er ) const; // returns the radius relative to the given center @@ -333,6 +335,10 @@ ID_INLINE idBounds idBounds::Translate( const idVec3 &translation ) const { return idBounds( b[0] + translation, b[1] + translation ); } +ID_INLINE idVec3 idBounds::Size(void) const { + return(b[1] - b[0]); +} + ID_INLINE idBounds &idBounds::TranslateSelf( const idVec3 &translation ) { b[0] += translation; b[1] += translation;