base
This commit is contained in:
+110
-88
@@ -349,108 +349,130 @@ bool TInTNoInfo(t& t1, t& t2) {
|
||||
//FINAL
|
||||
return true;
|
||||
}
|
||||
std::optional<TInTInfo> TInT(t& t1, t& t2) {
|
||||
std::vector<glm::vec3> worldvertices1 = {
|
||||
// Separating axis test between two boxes: the 3 face directions of each, and the 9 crossings of those.
|
||||
// The direction they overlap least along is the way to push them apart, and that overlap is how deep they are.
|
||||
// Each box's reach along a direction comes straight from its half sizes, no corners needed.
|
||||
std::optional<TInTInfo> TInT(const t& t1, const t& t2, bool withPoint) {
|
||||
const glm::vec3 axes1[3] = { t1.GetRightVector(), t1.GetUpVector(), t1.GetFrontVector() };
|
||||
const glm::vec3 axes2[3] = { t2.GetRightVector(), t2.GetUpVector(), t2.GetFrontVector() };
|
||||
const glm::vec3 half1 = t1.GetScale() * 0.5f;
|
||||
const glm::vec3 half2 = t2.GetScale() * 0.5f;
|
||||
const glm::vec3 center1 = t1.GetTranslation();
|
||||
const glm::vec3 center2 = t2.GetTranslation();
|
||||
|
||||
glm::vec3 axes[15];
|
||||
for (int i = 0; i < 3; i++) {
|
||||
axes[i] = axes1[i];
|
||||
axes[3 + i] = axes2[i];
|
||||
for (int j = 0; j < 3; j++)
|
||||
axes[6 + i * 3 + j] = glm::cross(axes1[i], axes2[j]);
|
||||
}
|
||||
|
||||
float lowestoverlap = FLT_MAX;
|
||||
glm::vec3 bestAxis(0.0f);
|
||||
bool bestInfront = false;
|
||||
|
||||
for (int i = 0; i < 15; i++) {
|
||||
glm::vec3 axis = axes[i];
|
||||
if (i >= 6) {
|
||||
const float len = glm::length(axis);
|
||||
if (len < 1e-6f) continue; // parallel edges, already covered by the face directions
|
||||
axis /= len;
|
||||
}
|
||||
|
||||
const float c1 = glm::dot(center1, axis);
|
||||
const float c2 = glm::dot(center2, axis);
|
||||
const float r1 = glm::abs(glm::dot(axes1[0], axis)) * half1.x + glm::abs(glm::dot(axes1[1], axis)) * half1.y
|
||||
+ glm::abs(glm::dot(axes1[2], axis)) * half1.z;
|
||||
const float r2 = glm::abs(glm::dot(axes2[0], axis)) * half2.x + glm::abs(glm::dot(axes2[1], axis)) * half2.y
|
||||
+ glm::abs(glm::dot(axes2[2], axis)) * half2.z;
|
||||
|
||||
const float min1 = c1 - r1, max1 = c1 + r1;
|
||||
const float min2 = c2 - r2, max2 = c2 + r2;
|
||||
if (!(min1 < max2 && min2 < max1))
|
||||
return {};
|
||||
|
||||
const float overlap = glm::min(max1, max2) - glm::max(min1, min2);
|
||||
if (overlap < lowestoverlap) {
|
||||
lowestoverlap = overlap;
|
||||
bestAxis = axis;
|
||||
bestInfront = c1 > c2;
|
||||
}
|
||||
}
|
||||
|
||||
TInTInfo tr;
|
||||
tr.overlap = lowestoverlap;
|
||||
tr.CN = bestAxis * (bestInfront ? 1.0f : -1.0f); // from t2 toward t1
|
||||
tr.POI = withPoint ? BoxContactPoint(t1, t2) : glm::vec3(0.0f);
|
||||
return tr;
|
||||
}
|
||||
|
||||
// Where two overlapping boxes touch: the middle of whichever corners are inside the other box,
|
||||
// or with none, the closest point between their edges
|
||||
glm::vec3 BoxContactPoint(const t& t1, const t& t2) {
|
||||
static const glm::vec3 corners[8] = {
|
||||
{0.5f,0.5f,0.5f},{0.5f,-0.5f,0.5f},{0.5f,0.5f,-0.5f},{0.5f,-0.5f,-0.5f},
|
||||
{-0.5f,0.5f,0.5f},{-0.5f,-0.5f,0.5f},{-0.5f,0.5f,-0.5f},{-0.5f,-0.5f,-0.5f},
|
||||
};
|
||||
std::vector<glm::vec3> worldvertices2 = worldvertices1;
|
||||
glm::mat4 t1mat = t1.GetMatrix(), t2mat = t2.GetMatrix();
|
||||
for (glm::vec3& v : worldvertices1) v = glm::vec3(t1mat * glm::vec4(v, 1.0f));
|
||||
for (glm::vec3& v : worldvertices2) v = glm::vec3(t2mat * glm::vec4(v, 1.0f));
|
||||
const glm::mat4 t1mat = t1.GetMatrix(), t2mat = t2.GetMatrix();
|
||||
const glm::mat4 t1inv = glm::inverse(t1mat), t2inv = glm::inverse(t2mat);
|
||||
|
||||
glm::vec3 fv1=t1.GetFrontVector(), rv1=t1.GetRightVector(), uv1=t1.GetUpVector();
|
||||
glm::vec3 fv2=t2.GetFrontVector(), rv2=t2.GetRightVector(), uv2=t2.GetUpVector();
|
||||
glm::vec3 worldvertices1[8], worldvertices2[8];
|
||||
for (int i = 0; i < 8; i++) {
|
||||
worldvertices1[i] = glm::vec3(t1mat * glm::vec4(corners[i], 1.0f));
|
||||
worldvertices2[i] = glm::vec3(t2mat * glm::vec4(corners[i], 1.0f));
|
||||
}
|
||||
|
||||
std::vector<glm::vec3> allAxes = {
|
||||
fv1, uv1, rv1, fv2, uv2, rv2,
|
||||
glm::cross(fv1,fv2), glm::cross(fv1,rv2), glm::cross(fv1,uv2),
|
||||
glm::cross(rv1,fv2), glm::cross(rv1,rv2), glm::cross(rv1,uv2),
|
||||
glm::cross(uv1,fv2), glm::cross(uv1,rv2), glm::cross(uv1,uv2)
|
||||
glm::vec3 summed(0.0f);
|
||||
int inside = 0;
|
||||
for (int i = 0; i < 8; i++) {
|
||||
if (glm::all(glm::lessThanEqual(glm::abs(glm::vec3(t1inv * glm::vec4(worldvertices2[i], 1.0f))), glm::vec3(0.5f + 1e-4f)))) {
|
||||
summed += worldvertices2[i];
|
||||
inside++;
|
||||
}
|
||||
if (glm::all(glm::lessThanEqual(glm::abs(glm::vec3(t2inv * glm::vec4(worldvertices1[i], 1.0f))), glm::vec3(0.5f + 1e-4f)))) {
|
||||
summed += worldvertices1[i];
|
||||
inside++;
|
||||
}
|
||||
}
|
||||
if (inside > 0)
|
||||
return summed / (float)inside;
|
||||
|
||||
static const int edgeIdx[][2] = {
|
||||
{0,1},{0,2},{1,3},{2,3},
|
||||
{4,5},{4,6},{5,7},{6,7},
|
||||
{0,4},{1,5},{2,6},{3,7}
|
||||
};
|
||||
|
||||
TInTInfo tr;
|
||||
float lowestoverlap = FLT_MAX;
|
||||
bool bestInfront = false;
|
||||
int bestIndex = 0;
|
||||
float bestDist = FLT_MAX;
|
||||
glm::vec3 bestPt(0.0f);
|
||||
|
||||
for (int i = 0; i < (int)allAxes.size(); i++) {
|
||||
glm::vec3 axis = allAxes[i];
|
||||
if (i >= 6) {
|
||||
float len = glm::length(axis);
|
||||
if (len < 1e-6f) continue;
|
||||
axis = axis / len;
|
||||
allAxes[i] = axis;
|
||||
}
|
||||
std::optional<AxisCollisionCN> b = TAxisCollidesTCN(worldvertices1, worldvertices2, axis);
|
||||
if (!b.has_value()) return {};
|
||||
if (b->overlap < lowestoverlap) {
|
||||
lowestoverlap = b->overlap;
|
||||
bestInfront = b->infront;
|
||||
bestIndex = i;
|
||||
}
|
||||
}
|
||||
for (auto& e1 : edgeIdx) {
|
||||
glm::vec3 p1 = worldvertices1[e1[0]], p2 = worldvertices1[e1[1]];
|
||||
for (auto& e2 : edgeIdx) {
|
||||
glm::vec3 p3 = worldvertices2[e2[0]], p4 = worldvertices2[e2[1]];
|
||||
|
||||
tr.overlap = lowestoverlap;
|
||||
tr.CN = allAxes[bestIndex] * (bestInfront ? 1.0f : -1.0f);
|
||||
|
||||
// POI
|
||||
glm::mat4 t1inv = glm::inverse(t1mat), t2inv = glm::inverse(t2mat);
|
||||
std::vector<glm::vec3> contactPts;
|
||||
for (const glm::vec3& v : worldvertices2) {
|
||||
glm::vec3 local = glm::vec3(t1inv * glm::vec4(v, 1.0f));
|
||||
if (glm::all(glm::lessThanEqual(glm::abs(local), glm::vec3(0.5f + 1e-4f))))
|
||||
contactPts.push_back(v);
|
||||
}
|
||||
for (const glm::vec3& v : worldvertices1) {
|
||||
glm::vec3 local = glm::vec3(t2inv * glm::vec4(v, 1.0f));
|
||||
if (glm::all(glm::lessThanEqual(glm::abs(local), glm::vec3(0.5f + 1e-4f))))
|
||||
contactPts.push_back(v);
|
||||
}
|
||||
|
||||
if (contactPts.empty()) {
|
||||
static const int edgeIdx[][2] = {
|
||||
{0,1},{0,2},{1,3},{2,3},
|
||||
{4,5},{4,6},{5,7},{6,7},
|
||||
{0,4},{1,5},{2,6},{3,7}
|
||||
};
|
||||
|
||||
float bestDist = FLT_MAX;
|
||||
glm::vec3 bestPt(0.0f);
|
||||
|
||||
for (auto& e1 : edgeIdx) {
|
||||
glm::vec3 p1 = worldvertices1[e1[0]], p2 = worldvertices1[e1[1]];
|
||||
for (auto& e2 : edgeIdx) {
|
||||
glm::vec3 p3 = worldvertices2[e2[0]], p4 = worldvertices2[e2[1]];
|
||||
|
||||
glm::vec3 d1=p2-p1, d2=p4-p3, r=p1-p3;
|
||||
float a=glm::dot(d1,d1), e=glm::dot(d2,d2), f=glm::dot(d2,r);
|
||||
float s, t;
|
||||
if (a < 1e-10f && e < 1e-10f) { s=0.0f; t=0.0f; }
|
||||
else if (a < 1e-10f) { s=0.0f; t=glm::clamp(f/e,0.0f,1.0f); }
|
||||
glm::vec3 d1=p2-p1, d2=p4-p3, r=p1-p3;
|
||||
float a=glm::dot(d1,d1), e=glm::dot(d2,d2), f=glm::dot(d2,r);
|
||||
float s, t;
|
||||
if (a < 1e-10f && e < 1e-10f) { s=0.0f; t=0.0f; }
|
||||
else if (a < 1e-10f) { s=0.0f; t=glm::clamp(f/e,0.0f,1.0f); }
|
||||
else {
|
||||
float c=glm::dot(d1,r), b=glm::dot(d1,d2), denom=a*e-b*b;
|
||||
if (e < 1e-10f) { t=0.0f; s=glm::clamp(-c/a,0.0f,1.0f); }
|
||||
else {
|
||||
float c=glm::dot(d1,r), b=glm::dot(d1,d2), denom=a*e-b*b;
|
||||
if (e < 1e-10f) { t=0.0f; s=glm::clamp(-c/a,0.0f,1.0f); }
|
||||
else {
|
||||
s = denom > 1e-10f ? glm::clamp((b*f-c*e)/denom,0.0f,1.0f) : 0.0f;
|
||||
t = (b*s+f)/e;
|
||||
if (t < 0.0f) { t=0.0f; s=glm::clamp(-c/a,0.0f,1.0f); }
|
||||
else if (t > 1.0f) { t=1.0f; s=glm::clamp((b-c)/a,0.0f,1.0f); }
|
||||
}
|
||||
s = denom > 1e-10f ? glm::clamp((b*f-c*e)/denom,0.0f,1.0f) : 0.0f;
|
||||
t = (b*s+f)/e;
|
||||
if (t < 0.0f) { t=0.0f; s=glm::clamp(-c/a,0.0f,1.0f); }
|
||||
else if (t > 1.0f) { t=1.0f; s=glm::clamp((b-c)/a,0.0f,1.0f); }
|
||||
}
|
||||
glm::vec3 c1=p1+s*d1, c2=p3+t*d2;
|
||||
float dist = glm::length(c1-c2);
|
||||
if (dist < bestDist) { bestDist=dist; bestPt=(c1+c2)*0.5f; }
|
||||
}
|
||||
glm::vec3 c1=p1+s*d1, c2=p3+t*d2;
|
||||
float dist = glm::dot(c1-c2, c1-c2);
|
||||
if (dist < bestDist) { bestDist=dist; bestPt=(c1+c2)*0.5f; }
|
||||
}
|
||||
tr.POI = bestPt;
|
||||
} else {
|
||||
tr.POI = glm::vec3(0.0f);
|
||||
for (const glm::vec3& p : contactPts) tr.POI += p;
|
||||
tr.POI /= (float)contactPts.size();
|
||||
}
|
||||
|
||||
return tr;
|
||||
return bestPt;
|
||||
}
|
||||
glm::quat AngleAxis(glm::vec3 axis, float angle) {
|
||||
float halfangle = angle * 0.5f;
|
||||
|
||||
Reference in New Issue
Block a user