Justin Marshall
774 lines
31 KiB
C++
774 lines
31 KiB
C++
#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 DEG2RAD = 0.01745329251994329577f;
|
|
constexpr float RAD2DEG = 57.295779513082320876f;
|
|
constexpr float ROTATION_EPSILON = 0.01f;
|
|
|
|
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 RotateAroundAxis(const idVec3& point, const idVec3& origin,
|
|
idVec3 axis, const float angleDegrees) {
|
|
if (axis.NormalizeFast() == 0.0f || angleDegrees == 0.0f) {
|
|
return point;
|
|
}
|
|
const float angle = angleDegrees * DEG2RAD;
|
|
const float sine = std::sin(angle);
|
|
const float cosine = std::cos(angle);
|
|
const idVec3 relative = point - origin;
|
|
return origin + relative * cosine + axis.Cross(relative) * sine
|
|
+ axis * (axis.Dot(relative) * (1.0f - cosine));
|
|
}
|
|
|
|
idVec3 RotateVector(const idVec3& value, idVec3 axis,
|
|
const float angleDegrees) {
|
|
return RotateAroundAxis(value, idVec3(0.0f, 0.0f, 0.0f),
|
|
axis, angleDegrees);
|
|
}
|
|
|
|
bool TestAndSet(std::uint8_t* bits, const int index) {
|
|
if (bits == nullptr) {
|
|
return false;
|
|
}
|
|
const std::uint8_t mask = static_cast<std::uint8_t>(1u << (index & 7));
|
|
std::uint8_t& value = bits[index >> 3];
|
|
const bool old = (value & mask) != 0;
|
|
value = static_cast<std::uint8_t>(value | mask);
|
|
return old;
|
|
}
|
|
|
|
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 positive = false;
|
|
bool negative = false;
|
|
for (int index = 0; index < polygon.numEdges; ++index) {
|
|
const std::uint16_t reference = model.polygonEdges[
|
|
polygon.firstEdge + index];
|
|
const cm_edge_t& edge = model.edges[CM_EdgeIndex(reference)];
|
|
const idVec3& start = model.vertices[
|
|
CM_EdgeStartVertex(edge, reference)].p;
|
|
const idVec3& end = model.vertices[
|
|
CM_EdgeEndVertex(edge, reference)].p;
|
|
const float side = (end - start).Cross(point - start).Dot(
|
|
plane.Normal());
|
|
positive |= side > ROTATION_EPSILON;
|
|
negative |= side < -ROTATION_EPSILON;
|
|
if (positive && negative) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void ClosestSegmentPoints(const idVec3& p1, const idVec3& q1,
|
|
const idVec3& p2, const idVec3& q2, idVec3& first, idVec3& second) {
|
|
const idVec3 d1 = q1 - p1;
|
|
const idVec3 d2 = q2 - p2;
|
|
const idVec3 r = p1 - p2;
|
|
const float a = d1.Dot(d1);
|
|
const float e = d2.Dot(d2);
|
|
const float f = d2.Dot(r);
|
|
float s = 0.0f;
|
|
float t = 0.0f;
|
|
if (a <= 1.0e-12f && e <= 1.0e-12f) {
|
|
first = p1;
|
|
second = p2;
|
|
return;
|
|
}
|
|
if (a <= 1.0e-12f) {
|
|
t = (std::max)(0.0f, (std::min)(1.0f, f / e));
|
|
} else {
|
|
const float c = d1.Dot(r);
|
|
if (e <= 1.0e-12f) {
|
|
s = (std::max)(0.0f, (std::min)(1.0f, -c / a));
|
|
} else {
|
|
const float b = d1.Dot(d2);
|
|
const float denominator = a * e - b * b;
|
|
if (std::fabs(denominator) > 1.0e-12f) {
|
|
s = (std::max)(0.0f, (std::min)(1.0f,
|
|
(b * f - c * e) / denominator));
|
|
}
|
|
t = (b * s + f) / e;
|
|
if (t < 0.0f) {
|
|
t = 0.0f;
|
|
s = (std::max)(0.0f, (std::min)(1.0f, -c / a));
|
|
} else if (t > 1.0f) {
|
|
t = 1.0f;
|
|
s = (std::max)(0.0f,
|
|
(std::min)(1.0f, (b - c) / a));
|
|
}
|
|
}
|
|
}
|
|
first = p1 + d1 * s;
|
|
second = p2 + d2 * t;
|
|
}
|
|
|
|
void StoreRotationCollision(idTraceWork& tw, const float fraction,
|
|
const contactType_t type, const idVec3& point, idVec3 normal,
|
|
const float distance, const cm_material_t& material,
|
|
const int modelFeature, const int trmFeature) {
|
|
if (tw.traceResult == nullptr || fraction >= tw.fraction) {
|
|
return;
|
|
}
|
|
if (normal.NormalizeFast() == 0.0f) {
|
|
return;
|
|
}
|
|
tw.fraction = (std::max)(0.0f, fraction);
|
|
trace_t& trace = *tw.traceResult;
|
|
trace.fraction = tw.fraction;
|
|
trace.c.type = type;
|
|
trace.c.point = point;
|
|
trace.c.normal = normal;
|
|
trace.c.dist = distance;
|
|
trace.c.separation = 0.0f;
|
|
SetMaterial(trace.c, material);
|
|
trace.c.modelFeature = modelFeature;
|
|
trace.c.trmFeature = trmFeature;
|
|
trace.c.flags = 0;
|
|
}
|
|
|
|
bool PointInsideInitialTrace(const idTraceWork& tw, const idVec3& point,
|
|
int& nearestPlane) {
|
|
float nearestDistance = -std::numeric_limits<float>::max();
|
|
nearestPlane = 0;
|
|
for (unsigned int index = 0; index < tw.numPolys; ++index) {
|
|
const float distance = tw.polys[index].plane.Distance(point);
|
|
if (distance > 0.0f) {
|
|
return false;
|
|
}
|
|
if (distance > nearestDistance) {
|
|
nearestDistance = distance;
|
|
nearestPlane = static_cast<int>(index);
|
|
}
|
|
}
|
|
return tw.numPolys != 0;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
float CM_TanZeroHalfPI(const float angle) {
|
|
const float clamped = (std::max)(0.0f,
|
|
(std::min)(1.57079632679f, angle));
|
|
return std::tan(clamped);
|
|
}
|
|
|
|
float CM_ArcTanPositive(const float value) {
|
|
return std::atan((std::max)(0.0f, value));
|
|
}
|
|
|
|
void CM_PointRotationBounds(const idVec3& origin, const idVec3& axis,
|
|
const idVec3& start, const idVec3& end, idVec4& boundsMin,
|
|
idVec4& boundsMax) {
|
|
idVec3 minimum((std::min)(start.x, end.x),
|
|
(std::min)(start.y, end.y), (std::min)(start.z, end.z));
|
|
idVec3 maximum((std::max)(start.x, end.x),
|
|
(std::max)(start.y, end.y), (std::max)(start.z, end.z));
|
|
// Include quarter turns around the recovered axis; this captures extrema
|
|
// missed by an endpoint-only arc bound.
|
|
for (int step = 1; step < 4; ++step) {
|
|
const idVec3 point = RotateAroundAxis(start, origin, axis,
|
|
static_cast<float>(step * 90));
|
|
minimum.x = (std::min)(minimum.x, point.x);
|
|
minimum.y = (std::min)(minimum.y, point.y);
|
|
minimum.z = (std::min)(minimum.z, point.z);
|
|
maximum.x = (std::max)(maximum.x, point.x);
|
|
maximum.y = (std::max)(maximum.y, point.y);
|
|
maximum.z = (std::max)(maximum.z, point.z);
|
|
}
|
|
boundsMin.Set(minimum.x, minimum.y, minimum.z, 0.0f);
|
|
boundsMax.Set(maximum.x, maximum.y, maximum.z, 0.0f);
|
|
}
|
|
|
|
bool idPolygonModelCollisionDetection::EdgeIntersectsBoundsShort(
|
|
const idBoundsShort& bounds, const idVec3& start, const idVec3& end) {
|
|
return bounds.ToBounds().LineIntersection(start, end);
|
|
}
|
|
|
|
void idPolygonModelCollisionDetection::RotationSetup(idTraceWork* const tw,
|
|
const idVec3& rotationOrigin, const idVec3& rotationAxis,
|
|
const float angle, const idVec3& start, const idVec3& offset,
|
|
const idMat3& trmAxis, const idVec3& modelOrigin,
|
|
const idMat3& modelAxis) {
|
|
const idVec3 centerWorld = start + ModelToWorldVector(trmAxis, offset);
|
|
const idVec3 localCenter = WorldToModelVector(modelAxis,
|
|
centerWorld - modelOrigin);
|
|
const idVec3 localOrigin = WorldToModelVector(modelAxis,
|
|
rotationOrigin - modelOrigin);
|
|
idVec3 localAxis = WorldToModelVector(modelAxis, rotationAxis);
|
|
localAxis.NormalizeFast();
|
|
SetVec4(tw->start, localCenter);
|
|
SetVec4(tw->origin, localOrigin);
|
|
SetVec4(tw->axis, localAxis);
|
|
tw->angle = angle;
|
|
tw->negAngle = -angle;
|
|
tw->initialTan = 0.0f;
|
|
tw->maxTan = std::tan(std::fabs(angle) * DEG2RAD * 0.5f);
|
|
const idVec3 endCenter = RotateAroundAxis(localCenter,
|
|
localOrigin, localAxis, angle);
|
|
SetVec4(tw->end, endCenter);
|
|
SetVec4(tw->dir, endCenter - localCenter);
|
|
SetVec4(tw->negDir, localCenter - endCenter);
|
|
}
|
|
|
|
void idPolygonModelCollisionDetection::TransformFromOriginAxisAngle(
|
|
idMat3x4& transform, const idVec3& origin, const idVec3& axis,
|
|
const float angle) {
|
|
const idVec3 x = RotateVector(idVec3(1.0f, 0.0f, 0.0f), axis, angle);
|
|
const idVec3 y = RotateVector(idVec3(0.0f, 1.0f, 0.0f), axis, angle);
|
|
const idVec3 z = RotateVector(idVec3(0.0f, 0.0f, 1.0f), axis, angle);
|
|
transform.mat[0] = x.x; transform.mat[1] = y.x; transform.mat[2] = z.x;
|
|
transform.mat[4] = x.y; transform.mat[5] = y.y; transform.mat[6] = z.y;
|
|
transform.mat[8] = x.z; transform.mat[9] = y.z; transform.mat[10] = z.z;
|
|
const idVec3 translated = origin - RotateVector(origin, axis, angle);
|
|
transform.mat[3] = translated.x;
|
|
transform.mat[7] = translated.y;
|
|
transform.mat[11] = translated.z;
|
|
}
|
|
|
|
void idPolygonModelCollisionDetection::TransformAxisToZAxis(
|
|
idMat3x4& transform, const idVec3& origin, const idVec3& axis) {
|
|
idVec3 z = axis;
|
|
if (z.NormalizeFast() == 0.0f) {
|
|
transform.Identity();
|
|
return;
|
|
}
|
|
idVec3 reference = std::fabs(z.z) < 0.9f
|
|
? idVec3(0.0f, 0.0f, 1.0f) : idVec3(0.0f, 1.0f, 0.0f);
|
|
idVec3 x = reference.Cross(z);
|
|
x.NormalizeFast();
|
|
idVec3 y = z.Cross(x);
|
|
y.NormalizeFast();
|
|
transform.mat[0] = x.x; transform.mat[1] = x.y; transform.mat[2] = x.z;
|
|
transform.mat[4] = y.x; transform.mat[5] = y.y; transform.mat[6] = y.z;
|
|
transform.mat[8] = z.x; transform.mat[9] = z.y; transform.mat[10] = z.z;
|
|
transform.mat[3] = -x.Dot(origin);
|
|
transform.mat[7] = -y.Dot(origin);
|
|
transform.mat[11] = -z.Dot(origin);
|
|
}
|
|
|
|
void idPolygonModelCollisionDetection::RotationVerts(idTraceWork* const tw,
|
|
const idTraceModel&, idVec4* const zverts) {
|
|
for (unsigned int index = 0; index < tw->numVerts; ++index) {
|
|
const idVec3 start = Vec3(tw->vertexPosition[index]);
|
|
const idVec3 end = RotateAroundAxis(start, Vec3(tw->origin),
|
|
Vec3(tw->axis), tw->angle);
|
|
SetVec4(tw->vertexEndPosition[index], end);
|
|
if (zverts != nullptr) {
|
|
idVec3 transformed;
|
|
tw->ZAxisTransform.Transform(transformed, start);
|
|
SetVec4(zverts[index], transformed);
|
|
}
|
|
}
|
|
}
|
|
|
|
void idPolygonModelCollisionDetection::RotationEdges(idTraceWork* const tw,
|
|
const idTraceModel&, const idVec4*) {
|
|
for (unsigned int index = 0; index < tw->numEdges; ++index) {
|
|
const idVec3 start = Vec3(tw->vertexPosition[
|
|
tw->edges[index].vertexNum[0]]);
|
|
const idVec3 end = Vec3(tw->vertexPosition[
|
|
tw->edges[index].vertexNum[1]]);
|
|
tw->edgePluecker[index].FromLine(start, end);
|
|
tw->edgeZAxisPluecker[index] = tw->edgePluecker[index];
|
|
}
|
|
}
|
|
|
|
void idPolygonModelCollisionDetection::RotationPolys(idTraceWork* const tw,
|
|
const idTraceModel&) {
|
|
for (unsigned int index = 0; index < tw->numPolys; ++index) {
|
|
tw->polyIsUsed[index] = tw->polys[index].numEdges != 0;
|
|
}
|
|
}
|
|
|
|
void idPolygonModelCollisionDetection::RotationBounds(idTraceWork* const tw) {
|
|
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());
|
|
const int steps = (std::max)(1,
|
|
static_cast<int>(std::ceil(std::fabs(tw->angle) / 10.0f)));
|
|
for (unsigned int vertex = 0; vertex < tw->numVerts; ++vertex) {
|
|
const idVec3 original = Vec3(tw->vertexPosition[vertex]);
|
|
for (int step = 0; step <= steps; ++step) {
|
|
const idVec3 point = RotateAroundAxis(original, Vec3(tw->origin),
|
|
Vec3(tw->axis), tw->angle * step / steps);
|
|
minimum.x = (std::min)(minimum.x, point.x);
|
|
minimum.y = (std::min)(minimum.y, point.y);
|
|
minimum.z = (std::min)(minimum.z, point.z);
|
|
maximum.x = (std::max)(maximum.x, point.x);
|
|
maximum.y = (std::max)(maximum.y, point.y);
|
|
maximum.z = (std::max)(maximum.z, point.z);
|
|
}
|
|
}
|
|
tw->traceBoundsMin.Set(minimum.x - 1.0f, minimum.y - 1.0f,
|
|
minimum.z - 1.0f, 0.0f);
|
|
tw->traceBoundsMax.Set(maximum.x + 1.0f, maximum.y + 1.0f,
|
|
maximum.z + 1.0f, 0.0f);
|
|
idBounds bounds;
|
|
bounds[0].Set(std::floor(minimum.x) - 1.0f,
|
|
std::floor(minimum.y) - 1.0f, std::floor(minimum.z) - 1.0f);
|
|
bounds[1].Set(std::ceil(maximum.x) + 1.0f,
|
|
std::ceil(maximum.y) + 1.0f, std::ceil(maximum.z) + 1.0f);
|
|
tw->traceBoundsShort.SetBounds(bounds);
|
|
}
|
|
|
|
void idPolygonModelCollisionDetection::RotationEdgePlueckerCache(
|
|
idTraceWork* const tw, const cm_polygon_t& polygon) {
|
|
TranslationPlueckerCache(tw, polygon);
|
|
}
|
|
|
|
void idPolygonModelCollisionDetection::RotationCullPolygonEdges(
|
|
idTraceWork* const tw, const cm_polygon_t& polygon) {
|
|
for (int index = 0; index < polygon.numEdges && index < 20; ++index) {
|
|
tw->polygonEdgeSideCache[index].side = 0;
|
|
const idPluecker& modelEdge = tw->polygonEdgePlueckerCache[index];
|
|
for (unsigned int trmEdge = 0; trmEdge < tw->numEdges; ++trmEdge) {
|
|
if (tw->edgePluecker[trmEdge] * modelEdge < 0.0f) {
|
|
tw->polygonEdgeSideCache[index].side |= 1u << trmEdge;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
int idPolygonModelCollisionDetection::CollisionBetweenEdgeBounds(
|
|
const idTraceWork&, const idVec3& firstStart, const idVec3& firstEnd,
|
|
const idVec3& secondStart, const idVec3& secondEnd, const float,
|
|
idVec3& collisionPoint, idVec3& collisionNormal) {
|
|
idVec3 firstPoint;
|
|
idVec3 secondPoint;
|
|
ClosestSegmentPoints(firstStart, firstEnd, secondStart, secondEnd,
|
|
firstPoint, secondPoint);
|
|
collisionNormal = firstPoint - secondPoint;
|
|
if (collisionNormal.LengthSqr() > ROTATION_EPSILON * ROTATION_EPSILON) {
|
|
return 0;
|
|
}
|
|
collisionPoint = (firstPoint + secondPoint) * 0.5f;
|
|
if (collisionNormal.NormalizeFast() == 0.0f) {
|
|
collisionNormal = (firstEnd - firstStart).Cross(
|
|
secondEnd - secondStart);
|
|
collisionNormal.NormalizeFast();
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
int idPolygonModelCollisionDetection::RotateEdgeThroughEdge(
|
|
const idPluecker& first, const idPluecker& second, const float angle,
|
|
const float minTan, const float maxTan, float& tanHalfAngle) {
|
|
idVec3 firstStart;
|
|
idVec3 firstEnd;
|
|
idVec3 secondStart;
|
|
idVec3 secondEnd;
|
|
if (!first.ToLine(firstStart, firstEnd)
|
|
|| !second.ToLine(secondStart, secondEnd)) {
|
|
return 0;
|
|
}
|
|
const int steps = (std::max)(8,
|
|
static_cast<int>(std::ceil(std::fabs(angle) / 5.0f)));
|
|
for (int step = 0; step <= steps; ++step) {
|
|
const float fraction = static_cast<float>(step) / steps;
|
|
const float tangent = std::tan(std::fabs(angle) * DEG2RAD
|
|
* fraction * 0.5f);
|
|
if (tangent < minTan || tangent > maxTan) {
|
|
continue;
|
|
}
|
|
const idVec3 rotatedStart = RotateAroundAxis(firstStart,
|
|
idVec3(0.0f, 0.0f, 0.0f), idVec3(0.0f, 0.0f, 1.0f),
|
|
angle * fraction);
|
|
const idVec3 rotatedEnd = RotateAroundAxis(firstEnd,
|
|
idVec3(0.0f, 0.0f, 0.0f), idVec3(0.0f, 0.0f, 1.0f),
|
|
angle * fraction);
|
|
idVec3 firstPoint;
|
|
idVec3 secondPoint;
|
|
ClosestSegmentPoints(rotatedStart, rotatedEnd,
|
|
secondStart, secondEnd, firstPoint, secondPoint);
|
|
if ((firstPoint - secondPoint).LengthSqr()
|
|
<= ROTATION_EPSILON * ROTATION_EPSILON) {
|
|
tanHalfAngle = tangent;
|
|
return 1;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int idPolygonModelCollisionDetection::EdgeFurthestFromEdge(
|
|
const idPluecker& first, const idPluecker& second, const float angle,
|
|
float& tanHalfAngle, float& direction) {
|
|
float bestDistance = -1.0f;
|
|
idVec3 firstStart;
|
|
idVec3 firstEnd;
|
|
idVec3 secondStart;
|
|
idVec3 secondEnd;
|
|
if (!first.ToLine(firstStart, firstEnd)
|
|
|| !second.ToLine(secondStart, secondEnd)) {
|
|
return 0;
|
|
}
|
|
for (int step = 0; step <= 32; ++step) {
|
|
const float fraction = step / 32.0f;
|
|
const idVec3 start = RotateAroundAxis(firstStart, idVec3(0, 0, 0),
|
|
idVec3(0, 0, 1), angle * fraction);
|
|
const idVec3 end = RotateAroundAxis(firstEnd, idVec3(0, 0, 0),
|
|
idVec3(0, 0, 1), angle * fraction);
|
|
idVec3 a;
|
|
idVec3 b;
|
|
ClosestSegmentPoints(start, end, secondStart, secondEnd, a, b);
|
|
const float distance = (a - b).LengthSqr();
|
|
if (distance > bestDistance) {
|
|
bestDistance = distance;
|
|
tanHalfAngle = std::tan(std::fabs(angle) * DEG2RAD
|
|
* fraction * 0.5f);
|
|
}
|
|
}
|
|
direction = angle < 0.0f ? -1.0f : 1.0f;
|
|
return bestDistance >= 0.0f;
|
|
}
|
|
|
|
int idPolygonModelCollisionDetection::RotateTrmEdgesThroughPolygon(
|
|
idTraceWork* const tw, const cm_polygon_t& polygon) {
|
|
const cm_material_t& material = tw->subModelPtrs.materials[polygon.material];
|
|
const int steps = (std::max)(8,
|
|
static_cast<int>(std::ceil(std::fabs(tw->angle) / 5.0f)));
|
|
for (unsigned int trmEdge = 0; trmEdge < tw->numEdges; ++trmEdge) {
|
|
const idVec3 originalStart = Vec3(tw->vertexPosition[
|
|
tw->edges[trmEdge].vertexNum[0]]);
|
|
const idVec3 originalEnd = Vec3(tw->vertexPosition[
|
|
tw->edges[trmEdge].vertexNum[1]]);
|
|
for (int modelEdgeNumber = 0;
|
|
modelEdgeNumber < polygon.numEdges; ++modelEdgeNumber) {
|
|
const std::uint16_t reference = tw->subModelPtrs.polygonEdges[
|
|
polygon.firstEdge + modelEdgeNumber];
|
|
const int modelEdge = CM_EdgeIndex(reference);
|
|
const cm_edge_t& edge = tw->subModelPtrs.edges[modelEdge];
|
|
const idVec3& modelStart = tw->subModelPtrs.vertices[
|
|
CM_EdgeStartVertex(edge, reference)].p;
|
|
const idVec3& modelEnd = tw->subModelPtrs.vertices[
|
|
CM_EdgeEndVertex(edge, reference)].p;
|
|
for (int step = 0; step <= steps; ++step) {
|
|
const float fraction = static_cast<float>(step) / steps;
|
|
if (fraction >= tw->fraction) {
|
|
break;
|
|
}
|
|
const idVec3 start = RotateAroundAxis(originalStart,
|
|
Vec3(tw->origin), Vec3(tw->axis), tw->angle * fraction);
|
|
const idVec3 end = RotateAroundAxis(originalEnd,
|
|
Vec3(tw->origin), Vec3(tw->axis), tw->angle * fraction);
|
|
idVec3 trmPoint;
|
|
idVec3 modelPoint;
|
|
ClosestSegmentPoints(start, end, modelStart, modelEnd,
|
|
trmPoint, modelPoint);
|
|
if ((trmPoint - modelPoint).LengthSqr()
|
|
> ROTATION_EPSILON * ROTATION_EPSILON) {
|
|
continue;
|
|
}
|
|
idVec3 normal = (end - start).Cross(modelEnd - modelStart);
|
|
StoreRotationCollision(*tw, fraction, CONTACT_EDGE,
|
|
(trmPoint + modelPoint) * 0.5f, normal,
|
|
normal.Dot(modelPoint), material,
|
|
((tw->subModelNum << 16) & 0x1FFF0000)
|
|
| 0x40000000 | modelEdge,
|
|
0x40000000 | static_cast<int>(trmEdge));
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
return tw->fraction <= 0.0f;
|
|
}
|
|
|
|
int idPolygonModelCollisionDetection::RotatePointThroughPlane(
|
|
const idVec3& point, const idPlane& plane, const float angle,
|
|
const float minTan, const float maxTan, float& tanHalfAngle) {
|
|
const int steps = (std::max)(8,
|
|
static_cast<int>(std::ceil(std::fabs(angle) / 5.0f)));
|
|
float previousDistance = plane.Distance(point);
|
|
for (int step = 1; step <= steps; ++step) {
|
|
const float fraction = static_cast<float>(step) / steps;
|
|
const float tangent = std::tan(std::fabs(angle) * DEG2RAD
|
|
* fraction * 0.5f);
|
|
const idVec3 rotated = RotateAroundAxis(point, idVec3(0, 0, 0),
|
|
idVec3(0, 0, 1), angle * fraction);
|
|
const float distance = plane.Distance(rotated);
|
|
if (tangent >= minTan && tangent <= maxTan
|
|
&& previousDistance > 0.0f && distance <= 0.0f) {
|
|
tanHalfAngle = tangent;
|
|
return 1;
|
|
}
|
|
previousDistance = distance;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int idPolygonModelCollisionDetection::PointFurthestFromPlane(
|
|
const idVec3& point, const idPlane& plane, const float angle,
|
|
float& tanHalfAngle, float& direction) {
|
|
float bestDistance = plane.Distance(point);
|
|
int bestStep = 0;
|
|
for (int step = 1; step <= 64; ++step) {
|
|
const idVec3 rotated = RotateAroundAxis(point, idVec3(0, 0, 0),
|
|
idVec3(0, 0, 1), angle * step / 64.0f);
|
|
const float distance = plane.Distance(rotated);
|
|
if (distance > bestDistance) {
|
|
bestDistance = distance;
|
|
bestStep = step;
|
|
}
|
|
}
|
|
tanHalfAngle = std::tan(std::fabs(angle) * DEG2RAD
|
|
* bestStep / 128.0f);
|
|
direction = angle < 0.0f ? -1.0f : 1.0f;
|
|
return bestStep != 0;
|
|
}
|
|
|
|
int idPolygonModelCollisionDetection::RotatePointThroughEpsilonPlane(
|
|
const idTraceWork& tw, const idVec3& point, const idVec3&,
|
|
const idPlane& plane, const float angle, const idVec3& rotationOrigin,
|
|
float& tanHalfAngle, idVec3& collisionPoint, idVec3& endDirection) {
|
|
const int steps = (std::max)(8,
|
|
static_cast<int>(std::ceil(std::fabs(angle) / 5.0f)));
|
|
float previousDistance = plane.Distance(point) - ROTATION_EPSILON;
|
|
for (int step = 1; step <= steps; ++step) {
|
|
const float fraction = static_cast<float>(step) / steps;
|
|
const idVec3 rotated = RotateAroundAxis(point, rotationOrigin,
|
|
Vec3(tw.axis), angle * fraction);
|
|
const float distance = plane.Distance(rotated) - ROTATION_EPSILON;
|
|
if (previousDistance > 0.0f && distance <= 0.0f) {
|
|
tanHalfAngle = std::tan(std::fabs(angle) * DEG2RAD
|
|
* fraction * 0.5f);
|
|
collisionPoint = rotated;
|
|
endDirection = Vec3(tw.axis).Cross(rotated - rotationOrigin);
|
|
return 1;
|
|
}
|
|
previousDistance = distance;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int idPolygonModelCollisionDetection::RotateTrmVertsThroughPolygon(
|
|
idTraceWork* const tw, const cm_polygon_t& polygon,
|
|
const idPlane& polygonPlane) {
|
|
const cm_material_t& material = tw->subModelPtrs.materials[polygon.material];
|
|
const int polygonNum = static_cast<int>(&polygon - tw->subModelPtrs.polygons);
|
|
const int steps = (std::max)(8,
|
|
static_cast<int>(std::ceil(std::fabs(tw->angle) / 5.0f)));
|
|
for (unsigned int vertex = 0; vertex < tw->numVerts; ++vertex) {
|
|
const idVec3 original = Vec3(tw->vertexPosition[vertex]);
|
|
idVec3 previous = original;
|
|
float previousDistance = polygonPlane.Distance(previous);
|
|
for (int step = 1; step <= steps; ++step) {
|
|
const float fraction = static_cast<float>(step) / steps;
|
|
if (fraction >= tw->fraction) {
|
|
break;
|
|
}
|
|
const idVec3 current = RotateAroundAxis(original,
|
|
Vec3(tw->origin), Vec3(tw->axis), tw->angle * fraction);
|
|
const float distance = polygonPlane.Distance(current);
|
|
if (previousDistance > 0.0f && distance <= 0.0f) {
|
|
float low = static_cast<float>(step - 1) / steps;
|
|
float high = fraction;
|
|
idVec3 collision = current;
|
|
for (int iteration = 0; iteration < 12; ++iteration) {
|
|
const float middle = (low + high) * 0.5f;
|
|
collision = RotateAroundAxis(original, Vec3(tw->origin),
|
|
Vec3(tw->axis), tw->angle * middle);
|
|
if (polygonPlane.Distance(collision) > 0.0f) {
|
|
low = middle;
|
|
} else {
|
|
high = middle;
|
|
}
|
|
}
|
|
if (PointInsidePolygon(tw->subModelPtrs, polygon,
|
|
polygonPlane, collision)) {
|
|
StoreRotationCollision(*tw, high, CONTACT_TRMVERTEX,
|
|
collision, polygonPlane.Normal(), polygonPlane.Dist(),
|
|
material, ((tw->subModelNum << 16) & 0x1FFF0000)
|
|
| 0x60000000 | polygonNum,
|
|
static_cast<int>(vertex));
|
|
}
|
|
}
|
|
previous = current;
|
|
previousDistance = distance;
|
|
}
|
|
}
|
|
return tw->fraction <= 0.0f;
|
|
}
|
|
|
|
int idPolygonModelCollisionDetection::RotatePolygonVertsThroughTrm(
|
|
idTraceWork* const tw, const cm_polygon_t& polygon) {
|
|
if (!tw->isConvex || tw->numPolys == 0) {
|
|
return 0;
|
|
}
|
|
const cm_material_t& material = tw->subModelPtrs.materials[polygon.material];
|
|
const int steps = (std::max)(8,
|
|
static_cast<int>(std::ceil(std::fabs(tw->angle) / 5.0f)));
|
|
for (int edgeNumber = 0; edgeNumber < polygon.numEdges; ++edgeNumber) {
|
|
const std::uint16_t reference = tw->subModelPtrs.polygonEdges[
|
|
polygon.firstEdge + edgeNumber];
|
|
const cm_edge_t& edge = tw->subModelPtrs.edges[CM_EdgeIndex(reference)];
|
|
const int vertexNumber = CM_EdgeStartVertex(edge, reference);
|
|
if (TestAndSet(tw->modelCheckCounts.vertexCheckCounts, vertexNumber)) {
|
|
continue;
|
|
}
|
|
const idVec3& modelPoint = tw->subModelPtrs.vertices[vertexNumber].p;
|
|
for (int step = 1; step <= steps; ++step) {
|
|
const float fraction = static_cast<float>(step) / steps;
|
|
if (fraction >= tw->fraction) {
|
|
break;
|
|
}
|
|
// Inverse-rotate the stationary model point into the trace model's
|
|
// initial frame, then test its recovered convex planes.
|
|
const idVec3 localPoint = RotateAroundAxis(modelPoint,
|
|
Vec3(tw->origin), Vec3(tw->axis), -tw->angle * fraction);
|
|
int nearestPlane = 0;
|
|
if (!PointInsideInitialTrace(*tw, localPoint, nearestPlane)) {
|
|
continue;
|
|
}
|
|
idVec3 normal = -RotateVector(tw->polys[nearestPlane].plane.Normal(),
|
|
Vec3(tw->axis), tw->angle * fraction);
|
|
StoreRotationCollision(*tw, fraction, CONTACT_MODELVERTEX,
|
|
modelPoint, normal, normal.Dot(modelPoint), material,
|
|
((tw->subModelNum << 16) & 0x1FFF0000)
|
|
| 0x20000000 | vertexNumber,
|
|
0x60000000 | nearestPlane);
|
|
break;
|
|
}
|
|
}
|
|
return tw->fraction <= 0.0f;
|
|
}
|
|
|
|
bool idPolygonModelCollisionDetection::RotateTrmThroughPolygon(
|
|
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
|
|
|| !tw->traceBoundsShort.IntersectsBounds(polygon.bounds)) {
|
|
return false;
|
|
}
|
|
idPlane plane;
|
|
CM_GetPolygonPlane(tw->subModelPtrs, polygon, plane);
|
|
RotationEdgePlueckerCache(tw, polygon);
|
|
RotationCullPolygonEdges(tw, polygon);
|
|
if (RotateTrmVertsThroughPolygon(tw, polygon, plane)
|
|
|| RotateTrmEdgesThroughPolygon(tw, polygon)
|
|
|| RotatePolygonVertsThroughTrm(tw, polygon)) {
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
int idPolygonModelCollisionDetection::StartRotation(idTraceWork* const tw,
|
|
trace_t* const result, const idVec3& rotationOrigin,
|
|
const idVec3& rotationAxis, const float angle, const idVec3& start,
|
|
const idTraceModel* const trm, const idMat3& trmAxis,
|
|
const int contentMask, const idVec3& modelOrigin,
|
|
const idMat3& modelAxis) {
|
|
if (trm == nullptr) {
|
|
return StartRotationPoint(tw, result, rotationOrigin, rotationAxis,
|
|
angle, start, contentMask, modelOrigin, modelAxis);
|
|
}
|
|
StartContents(tw, result, start, trm, trmAxis, contentMask,
|
|
modelOrigin, modelAxis);
|
|
tw->traceType = TRACE_ROTATION;
|
|
tw->fraction = 1.0f;
|
|
result->fraction = 1.0f;
|
|
RotationSetup(tw, rotationOrigin, rotationAxis, angle, start,
|
|
trm->offset, trmAxis, modelOrigin, modelAxis);
|
|
TransformAxisToZAxis(tw->ZAxisTransform, Vec3(tw->origin),
|
|
Vec3(tw->axis));
|
|
TransformFromOriginAxisAngle(tw->endTransform, Vec3(tw->origin),
|
|
Vec3(tw->axis), angle);
|
|
idVec4 zverts[32];
|
|
RotationVerts(tw, *trm, zverts);
|
|
RotationEdges(tw, *trm, zverts);
|
|
RotationPolys(tw, *trm);
|
|
RotationBounds(tw);
|
|
return angle != 0.0f;
|
|
}
|
|
|
|
int idPolygonModelCollisionDetection::StartRotationPoint(
|
|
idTraceWork* const tw, trace_t* const result,
|
|
const idVec3& rotationOrigin, const idVec3& rotationAxis,
|
|
const float angle, const idVec3& start, const int contentMask,
|
|
const idVec3& modelOrigin, const idMat3& modelAxis) {
|
|
StartContentsPoint(tw, result, start, contentMask, modelOrigin, modelAxis);
|
|
tw->traceType = TRACE_ROTATION_POINT;
|
|
RotationSetup(tw, rotationOrigin, rotationAxis, angle, start,
|
|
idVec3(0.0f, 0.0f, 0.0f), idMat3(1.0f),
|
|
modelOrigin, modelAxis);
|
|
tw->vertexPosition[0] = tw->start;
|
|
tw->vertexEndPosition[0] = tw->end;
|
|
RotationBounds(tw);
|
|
return angle != 0.0f;
|
|
}
|
|
|
|
void idPolygonModelCollisionDetection::FinishRotation(idTraceWork* const tw,
|
|
const idVec3& rotationOrigin, const idVec3& rotationAxis,
|
|
const float angle, const idVec3& start, const idMat3& trmAxis,
|
|
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) {
|
|
return;
|
|
}
|
|
trace_t& trace = *tw->traceResult;
|
|
trace.fraction = tw->fraction;
|
|
trace.endpos = RotateAroundAxis(start, rotationOrigin, rotationAxis,
|
|
angle * tw->fraction);
|
|
for (int column = 0; column < 3; ++column) {
|
|
trace.endAxis[column] = RotateVector(trmAxis[column], rotationAxis,
|
|
angle * tw->fraction);
|
|
}
|
|
if (trace.fraction >= 1.0f) {
|
|
return;
|
|
}
|
|
trace.c.normal = ModelToWorldVector(modelAxis, trace.c.normal);
|
|
trace.c.point = ModelToWorldVector(modelAxis, trace.c.point)
|
|
+ modelOrigin;
|
|
trace.c.dist += modelOrigin.Dot(trace.c.normal);
|
|
trace.c.entityNum = modelEntityNum;
|
|
trace.c.physicsId = modelPhysicsId;
|
|
trace.c.bodyId = modelBodyId;
|
|
trace.c.selfId = selfId;
|
|
if (modelContentsOverride != 0 && trace.c.contentFlags != 0) {
|
|
trace.c.contentFlags = modelContentsOverride;
|
|
}
|
|
}
|