Added collision model code.
This commit is contained in:
Justin Marshall
@@ -0,0 +1,447 @@
|
||||
#include "cm/jobs/polygonmodel/polygonmodel.h"
|
||||
|
||||
#include "cm/jobs/polygonmodel/polygonmodeldata.h"
|
||||
#include "idlib/geometry/tracemodel.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <cstring>
|
||||
#include <limits>
|
||||
|
||||
namespace {
|
||||
|
||||
constexpr float CM_BOUNDS_EPSILON = 1.0f;
|
||||
constexpr float CM_PLANE_EPSILON = 0.0001f;
|
||||
|
||||
idVec3 Vec3(const idVec4& value) {
|
||||
return idVec3(value.x, value.y, value.z);
|
||||
}
|
||||
|
||||
void SetVec4(idVec4& target, const idVec3& value, const float w = 0.0f) {
|
||||
target.Set(value.x, value.y, value.z, w);
|
||||
}
|
||||
|
||||
idVec3 ModelToWorldVector(const idMat3& axis, const idVec3& value) {
|
||||
return idVec3(
|
||||
axis[0].x * value.x + axis[1].x * value.y + axis[2].x * value.z,
|
||||
axis[0].y * value.x + axis[1].y * value.y + axis[2].y * value.z,
|
||||
axis[0].z * value.x + axis[1].z * value.y + axis[2].z * value.z);
|
||||
}
|
||||
|
||||
idVec3 WorldToModelVector(const idMat3& axis, const idVec3& value) {
|
||||
return idVec3(axis[0].Dot(value), axis[1].Dot(value),
|
||||
axis[2].Dot(value));
|
||||
}
|
||||
|
||||
idVec3 TraceModelToWorldVector(const idMat3& axis, const idVec3& value) {
|
||||
return ModelToWorldVector(axis, value);
|
||||
}
|
||||
|
||||
bool IsIdentity(const idMat3& axis) {
|
||||
return axis[0].x == 1.0f && axis[1].y == 1.0f
|
||||
&& axis[2].z == 1.0f
|
||||
&& axis[0].y == 0.0f && axis[0].z == 0.0f
|
||||
&& axis[1].x == 0.0f && axis[1].z == 0.0f
|
||||
&& axis[2].x == 0.0f && axis[2].y == 0.0f;
|
||||
}
|
||||
|
||||
bool BoundsIntersect(const idBoundsShort& lhs, const idBoundsShort& rhs) {
|
||||
return lhs.IntersectsBounds(rhs);
|
||||
}
|
||||
|
||||
bool TestAndSet(std::uint8_t* bits, const int index) {
|
||||
if (bits == nullptr || index < 0) {
|
||||
return false;
|
||||
}
|
||||
const std::uint8_t mask = static_cast<std::uint8_t>(1u << (index & 7));
|
||||
std::uint8_t& value = bits[index >> 3];
|
||||
const bool wasSet = (value & mask) != 0;
|
||||
value = static_cast<std::uint8_t>(value | mask);
|
||||
return wasSet;
|
||||
}
|
||||
|
||||
void SetMaterial(contactInfo_t& contact, const cm_material_t& material) {
|
||||
contact.contentFlags = material.contentFlags;
|
||||
contact.surfaceFlags = material.surfaceFlags;
|
||||
contact.surfaceType = material.surfaceType;
|
||||
contact.surfaceColor[0] = material.surfaceColor[0];
|
||||
contact.surfaceColor[1] = material.surfaceColor[1];
|
||||
contact.surfaceColor[2] = material.surfaceColor[2];
|
||||
}
|
||||
|
||||
bool PointInsidePolygon(const cm_subModelPtrs_t& model,
|
||||
const cm_polygon_t& polygon, const idPlane& plane, const idVec3& point) {
|
||||
bool hasPositive = false;
|
||||
bool hasNegative = false;
|
||||
const idVec3 normal = plane.Normal();
|
||||
for (int edgeNumber = 0; edgeNumber < polygon.numEdges; ++edgeNumber) {
|
||||
const std::uint16_t edgeReference =
|
||||
model.polygonEdges[polygon.firstEdge + edgeNumber];
|
||||
const cm_edge_t& edge = model.edges[CM_EdgeIndex(edgeReference)];
|
||||
const idVec3& start = model.vertices[
|
||||
CM_EdgeStartVertex(edge, edgeReference)].p;
|
||||
const idVec3& end = model.vertices[
|
||||
CM_EdgeEndVertex(edge, edgeReference)].p;
|
||||
const float side = (end - start).Cross(point - start).Dot(normal);
|
||||
hasPositive |= side > CM_PLANE_EPSILON;
|
||||
hasNegative |= side < -CM_PLANE_EPSILON;
|
||||
if (hasPositive && hasNegative) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool PointInsideTraceModel(const idTraceWork& tw, const idVec3& point,
|
||||
int& nearestPolygon) {
|
||||
float nearestDistance = -std::numeric_limits<float>::max();
|
||||
nearestPolygon = 0;
|
||||
for (unsigned int polygonNumber = 0;
|
||||
polygonNumber < tw.numPolys; ++polygonNumber) {
|
||||
const float distance = tw.polys[polygonNumber].plane.Distance(point);
|
||||
if (distance >= 0.0f) {
|
||||
return false;
|
||||
}
|
||||
if (distance > nearestDistance) {
|
||||
nearestDistance = distance;
|
||||
nearestPolygon = static_cast<int>(polygonNumber);
|
||||
}
|
||||
}
|
||||
return tw.numPolys != 0;
|
||||
}
|
||||
|
||||
void MakeTraceBounds(idTraceWork& tw, const idVec3& minimum,
|
||||
const idVec3& maximum, const bool padBounds) {
|
||||
const float padding = padBounds ? CM_BOUNDS_EPSILON : 0.0f;
|
||||
tw.traceBoundsMin.Set(minimum.x - padding, minimum.y - padding,
|
||||
minimum.z - padding, 0.0f);
|
||||
tw.traceBoundsMax.Set(maximum.x + padding, maximum.y + padding,
|
||||
maximum.z + padding, 0.0f);
|
||||
idBounds quantized;
|
||||
quantized[0].Set(std::floor(tw.traceBoundsMin.x) - 1.0f,
|
||||
std::floor(tw.traceBoundsMin.y) - 1.0f,
|
||||
std::floor(tw.traceBoundsMin.z) - 1.0f);
|
||||
quantized[1].Set(std::ceil(tw.traceBoundsMax.x) + 1.0f,
|
||||
std::ceil(tw.traceBoundsMax.y) + 1.0f,
|
||||
std::ceil(tw.traceBoundsMax.z) + 1.0f);
|
||||
tw.traceBoundsShort.SetBounds(quantized);
|
||||
}
|
||||
|
||||
void SetPolygonContact(idTraceWork& tw, const cm_polygon_t& polygon,
|
||||
const int polygonNum, const int trmVertex, const idVec3& point,
|
||||
const idPlane& plane) {
|
||||
trace_t& trace = *tw.traceResult;
|
||||
trace.fraction = 0.0f;
|
||||
trace.c.type = CONTACT_TRMVERTEX;
|
||||
trace.c.point = point;
|
||||
trace.c.normal = plane.Normal();
|
||||
trace.c.dist = plane.Dist();
|
||||
trace.c.separation = 0.0f;
|
||||
SetMaterial(trace.c, tw.subModelPtrs.materials[polygon.material]);
|
||||
trace.c.modelFeature = ((tw.subModelNum << 16) & 0x1FFF0000)
|
||||
| 0x60000000 | polygonNum;
|
||||
trace.c.trmFeature = trmVertex;
|
||||
trace.c.flags = 0;
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
bool idPolygonModelCollisionDetection::TestTrmVertsInPolytope(
|
||||
idTraceWork* const tw, const int polytopeNum) {
|
||||
if (TestAndSet(tw->modelCheckCounts.polytopeCheckCounts, polytopeNum)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const cm_polytope_t& polytope = tw->subModelPtrs.polytopes[polytopeNum];
|
||||
const cm_material_t& material =
|
||||
tw->subModelPtrs.materials[polytope.material];
|
||||
if ((material.contentFlags & tw->contents) == 0
|
||||
|| !BoundsIntersect(tw->traceBoundsShort, polytope.bounds)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
for (unsigned int vertexNumber = 0;
|
||||
vertexNumber < tw->numVerts; ++vertexNumber) {
|
||||
const idVec3 vertex = Vec3(tw->vertexPosition[vertexNumber]);
|
||||
float nearestDistance = -std::numeric_limits<float>::max();
|
||||
int nearestPlane = 0;
|
||||
bool inside = true;
|
||||
for (int planeNumber = 0;
|
||||
planeNumber < polytope.numPlanes; ++planeNumber) {
|
||||
const idPlane& plane = tw->subModelPtrs.polytopePlanes[
|
||||
polytope.firstPlane + planeNumber];
|
||||
const float distance = plane.Distance(vertex);
|
||||
if (distance >= 0.0f) {
|
||||
inside = false;
|
||||
break;
|
||||
}
|
||||
if (distance > nearestDistance) {
|
||||
nearestDistance = distance;
|
||||
nearestPlane = planeNumber;
|
||||
}
|
||||
}
|
||||
if (!inside) {
|
||||
continue;
|
||||
}
|
||||
|
||||
trace_t& trace = *tw->traceResult;
|
||||
const idPlane& plane = tw->subModelPtrs.polytopePlanes[
|
||||
polytope.firstPlane + nearestPlane];
|
||||
trace.fraction = 0.0f;
|
||||
trace.c.type = CONTACT_TRMVERTEX;
|
||||
trace.c.normal = plane.Normal();
|
||||
trace.c.dist = plane.Dist();
|
||||
trace.c.separation = 0.0f;
|
||||
trace.c.point = vertex;
|
||||
SetMaterial(trace.c, material);
|
||||
trace.c.modelFeature = 0x80000000
|
||||
| ((tw->subModelNum << 16) & 0x1FFF0000) | polytopeNum;
|
||||
trace.c.trmFeature = static_cast<int>(vertexNumber);
|
||||
trace.c.flags = 0;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool idPolygonModelCollisionDetection::TestTrmInPolygon(
|
||||
idTraceWork* const tw, const int polygonNum) {
|
||||
if (TestAndSet(tw->modelCheckCounts.polygonCheckCounts, polygonNum)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const cm_polygon_t& polygon = tw->subModelPtrs.polygons[polygonNum];
|
||||
const cm_material_t& material =
|
||||
tw->subModelPtrs.materials[polygon.material];
|
||||
if ((material.contentFlags & tw->contents) == 0
|
||||
|| !BoundsIntersect(tw->traceBoundsShort, polygon.bounds)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
idPlane polygonPlane;
|
||||
CM_GetPolygonPlane(tw->subModelPtrs, polygon, polygonPlane);
|
||||
bool hasFront = false;
|
||||
bool hasBack = false;
|
||||
for (unsigned int vertexNumber = 0;
|
||||
vertexNumber < tw->numVerts; ++vertexNumber) {
|
||||
const float distance = polygonPlane.Distance(
|
||||
Vec3(tw->vertexPosition[vertexNumber]));
|
||||
hasFront |= distance > CM_PLANE_EPSILON;
|
||||
hasBack |= distance < -CM_PLANE_EPSILON;
|
||||
}
|
||||
if (!hasBack) {
|
||||
if (tw->subModelPtrs.isConvex != 0) {
|
||||
// The recovered convex early-out terminates the whole submodel as
|
||||
// soon as one separating polygon plane is found.
|
||||
tw->quickExit = true;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
// The authoritative Pluecker tests find the same geometric event: a trace
|
||||
// edge crosses the model polygon. The PC scalar path spells it directly.
|
||||
for (unsigned int edgeNumber = 0; edgeNumber < tw->numEdges; ++edgeNumber) {
|
||||
const int first = tw->edges[edgeNumber].vertexNum[0];
|
||||
const int second = tw->edges[edgeNumber].vertexNum[1];
|
||||
const idVec3 firstPoint = Vec3(tw->vertexPosition[first]);
|
||||
const idVec3 secondPoint = Vec3(tw->vertexPosition[second]);
|
||||
const float firstDistance = polygonPlane.Distance(firstPoint);
|
||||
const float secondDistance = polygonPlane.Distance(secondPoint);
|
||||
if ((firstDistance < 0.0f) == (secondDistance < 0.0f)) {
|
||||
continue;
|
||||
}
|
||||
const float denominator = firstDistance - secondDistance;
|
||||
if (std::fabs(denominator) <= CM_PLANE_EPSILON) {
|
||||
continue;
|
||||
}
|
||||
const float fraction = firstDistance / denominator;
|
||||
const idVec3 point = firstPoint + (secondPoint - firstPoint) * fraction;
|
||||
if (PointInsidePolygon(tw->subModelPtrs, polygon,
|
||||
polygonPlane, point)) {
|
||||
const int featureVertex = firstDistance < 0.0f ? first : second;
|
||||
SetPolygonContact(*tw, polygon, polygonNum, featureVertex,
|
||||
Vec3(tw->vertexPosition[featureVertex]), polygonPlane);
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
// A model vertex can be embedded in the trace model without an edge-plane
|
||||
// crossing (the second recovered contents case).
|
||||
for (int edgeNumber = 0; edgeNumber < polygon.numEdges; ++edgeNumber) {
|
||||
const std::uint16_t edgeReference =
|
||||
tw->subModelPtrs.polygonEdges[polygon.firstEdge + edgeNumber];
|
||||
const cm_edge_t& edge =
|
||||
tw->subModelPtrs.edges[CM_EdgeIndex(edgeReference)];
|
||||
const int vertexNumber = CM_EdgeStartVertex(edge, edgeReference);
|
||||
if (TestAndSet(tw->modelCheckCounts.vertexCheckCounts, vertexNumber)) {
|
||||
continue;
|
||||
}
|
||||
const idVec3& point = tw->subModelPtrs.vertices[vertexNumber].p;
|
||||
int tracePolygon = 0;
|
||||
if (!PointInsideTraceModel(*tw, point, tracePolygon)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
trace_t& trace = *tw->traceResult;
|
||||
const idPlane& plane = tw->polys[tracePolygon].plane;
|
||||
trace.fraction = 0.0f;
|
||||
trace.c.type = CONTACT_MODELVERTEX;
|
||||
trace.c.point = point;
|
||||
trace.c.normal = -plane.Normal();
|
||||
trace.c.dist = plane.d;
|
||||
trace.c.separation = 0.0f;
|
||||
SetMaterial(trace.c, material);
|
||||
trace.c.modelFeature = ((tw->subModelNum << 16) & 0x1FFF0000)
|
||||
| 0x20000000 | vertexNumber;
|
||||
trace.c.trmFeature = tracePolygon;
|
||||
trace.c.flags = 0;
|
||||
return true;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
void idPolygonModelCollisionDetection::StartContents(idTraceWork* const tw,
|
||||
trace_t* const result, const idVec3& start, const idTraceModel* const trm,
|
||||
const idMat3& trmAxis, const int contentMask,
|
||||
const idVec3& modelOrigin, const idMat3& modelAxis) {
|
||||
std::memset(result, 0, sizeof(*result));
|
||||
result->fraction = 1.0f;
|
||||
result->endpos = start;
|
||||
result->endAxis = trmAxis;
|
||||
tw->traceResult = result;
|
||||
tw->contactsResult = nullptr;
|
||||
tw->clipResult = nullptr;
|
||||
tw->fraction = 1.0f;
|
||||
tw->contents = contentMask;
|
||||
tw->isConvex = trm->isConvex;
|
||||
tw->traceType = TRACE_CONTENTS;
|
||||
tw->quickExit = false;
|
||||
|
||||
const idVec3 localStart = WorldToModelVector(modelAxis,
|
||||
start + TraceModelToWorldVector(trmAxis, trm->offset) - modelOrigin);
|
||||
SetVec4(tw->start, localStart);
|
||||
tw->end = tw->start;
|
||||
|
||||
idVec3 minimum(std::numeric_limits<float>::max(),
|
||||
std::numeric_limits<float>::max(),
|
||||
std::numeric_limits<float>::max());
|
||||
idVec3 maximum(-std::numeric_limits<float>::max(),
|
||||
-std::numeric_limits<float>::max(),
|
||||
-std::numeric_limits<float>::max());
|
||||
tw->numVerts = (std::min)(trm->numVerts, 32u);
|
||||
for (unsigned int vertexNumber = 0;
|
||||
vertexNumber < tw->numVerts; ++vertexNumber) {
|
||||
const idVec3 source(trm->vertsX[vertexNumber],
|
||||
trm->vertsY[vertexNumber], trm->vertsZ[vertexNumber]);
|
||||
const idVec3 world = start + TraceModelToWorldVector(trmAxis, source);
|
||||
const idVec3 local = WorldToModelVector(modelAxis,
|
||||
world - modelOrigin);
|
||||
SetVec4(tw->vertexPosition[vertexNumber], local);
|
||||
minimum.x = (std::min)(minimum.x, local.x);
|
||||
minimum.y = (std::min)(minimum.y, local.y);
|
||||
minimum.z = (std::min)(minimum.z, local.z);
|
||||
maximum.x = (std::max)(maximum.x, local.x);
|
||||
maximum.y = (std::max)(maximum.y, local.y);
|
||||
maximum.z = (std::max)(maximum.z, local.z);
|
||||
}
|
||||
SetVec4(tw->trmBoundsMin, minimum - localStart);
|
||||
SetVec4(tw->trmBoundsMax, maximum - localStart);
|
||||
tw->trmExtents.Set(
|
||||
(std::max)(std::fabs(tw->trmBoundsMin.x),
|
||||
std::fabs(tw->trmBoundsMax.x)),
|
||||
(std::max)(std::fabs(tw->trmBoundsMin.y),
|
||||
std::fabs(tw->trmBoundsMax.y)),
|
||||
(std::max)(std::fabs(tw->trmBoundsMin.z),
|
||||
std::fabs(tw->trmBoundsMax.z)), 0.0f);
|
||||
|
||||
tw->numEdges = (std::min)(trm->numEdges, 32u);
|
||||
for (unsigned int edgeNumber = 0;
|
||||
edgeNumber < tw->numEdges; ++edgeNumber) {
|
||||
tw->edges[edgeNumber].vertexNum[0] = trm->edges[edgeNumber].v[0];
|
||||
tw->edges[edgeNumber].vertexNum[1] = trm->edges[edgeNumber].v[1];
|
||||
const idVec3 first = Vec3(tw->vertexPosition[
|
||||
tw->edges[edgeNumber].vertexNum[0]]);
|
||||
const idVec3 second = Vec3(tw->vertexPosition[
|
||||
tw->edges[edgeNumber].vertexNum[1]]);
|
||||
tw->edgePluecker[edgeNumber].FromLine(first, second);
|
||||
}
|
||||
|
||||
tw->numPolys = (std::min)(trm->numPolys, 16u);
|
||||
for (unsigned int polygonNumber = 0;
|
||||
polygonNumber < tw->numPolys; ++polygonNumber) {
|
||||
cm_trmPolygon_t& polygon = tw->polys[polygonNumber];
|
||||
polygon.numEdges = (std::min)(trm->numPolyEdges[polygonNumber], 16u);
|
||||
std::memcpy(polygon.edges, trm->polyEdges[polygonNumber],
|
||||
polygon.numEdges);
|
||||
const idVec3 sourceNormal(trm->polyPlaneX[polygonNumber],
|
||||
trm->polyPlaneY[polygonNumber], trm->polyPlaneZ[polygonNumber]);
|
||||
idVec3 localNormal = WorldToModelVector(modelAxis,
|
||||
TraceModelToWorldVector(trmAxis, sourceNormal));
|
||||
localNormal.NormalizeFast();
|
||||
polygon.plane.Normal() = localNormal;
|
||||
if (polygon.numEdges != 0) {
|
||||
const int edgeIndex = polygon.edges[0] & 0x7F;
|
||||
const int vertexIndex = tw->edges[edgeIndex].vertexNum[
|
||||
polygon.edges[0] >> 7];
|
||||
polygon.plane.d = -localNormal.Dot(
|
||||
Vec3(tw->vertexPosition[vertexIndex]));
|
||||
} else {
|
||||
polygon.plane.d = 0.0f;
|
||||
}
|
||||
}
|
||||
MakeTraceBounds(*tw, minimum, maximum, true);
|
||||
}
|
||||
|
||||
void idPolygonModelCollisionDetection::StartContentsPoint(
|
||||
idTraceWork* const tw, trace_t* const result, const idVec3& start,
|
||||
const int contentMask, const idVec3& modelOrigin,
|
||||
const idMat3& modelAxis) {
|
||||
std::memset(result, 0, sizeof(*result));
|
||||
result->fraction = 1.0f;
|
||||
result->endpos = start;
|
||||
result->endAxis = idMat3(1.0f);
|
||||
tw->traceResult = result;
|
||||
tw->contactsResult = nullptr;
|
||||
tw->clipResult = nullptr;
|
||||
tw->fraction = 1.0f;
|
||||
tw->contents = contentMask;
|
||||
tw->isConvex = true;
|
||||
tw->traceType = TRACE_CONTENTS_POINT;
|
||||
tw->quickExit = false;
|
||||
|
||||
const idVec3 local = WorldToModelVector(modelAxis, start - modelOrigin);
|
||||
SetVec4(tw->start, local);
|
||||
tw->end = tw->start;
|
||||
tw->numVerts = 1;
|
||||
tw->numEdges = 0;
|
||||
tw->numPolys = 0;
|
||||
tw->vertexPosition[0] = tw->start;
|
||||
tw->trmBoundsMin.Set(0.0f, 0.0f, 0.0f, 0.0f);
|
||||
tw->trmBoundsMax.Set(0.0f, 0.0f, 0.0f, 0.0f);
|
||||
tw->trmExtents.Set(0.0f, 0.0f, 0.0f, 0.0f);
|
||||
MakeTraceBounds(*tw, local, local, false);
|
||||
}
|
||||
|
||||
void idPolygonModelCollisionDetection::FinishContents(
|
||||
idTraceWork* const tw, const idVec3& modelOrigin,
|
||||
const idMat3& modelAxis, const int modelEntityNum,
|
||||
const int modelPhysicsId, const int modelBodyId, const int selfId,
|
||||
const int modelContentsOverride) {
|
||||
if (tw->traceResult == nullptr || tw->traceResult->fraction >= 1.0f) {
|
||||
return;
|
||||
}
|
||||
contactInfo_t& contact = tw->traceResult->c;
|
||||
if (!IsIdentity(modelAxis)) {
|
||||
contact.normal = ModelToWorldVector(modelAxis, contact.normal);
|
||||
contact.point = ModelToWorldVector(modelAxis, contact.point);
|
||||
}
|
||||
contact.point = contact.point + modelOrigin;
|
||||
contact.dist += modelOrigin.Dot(contact.normal);
|
||||
contact.entityNum = modelEntityNum;
|
||||
contact.physicsId = modelPhysicsId;
|
||||
contact.bodyId = modelBodyId;
|
||||
contact.selfId = selfId;
|
||||
if (modelContentsOverride != 0 && contact.contentFlags != 0) {
|
||||
contact.contentFlags = modelContentsOverride;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user