Skip to content

Commit

Permalink
add. 8942a2
Browse files Browse the repository at this point in the history
  • Loading branch information
rt committed Nov 23, 2018
1 parent b7bb119 commit 871cc9b
Showing 1 changed file with 54 additions and 54 deletions.
108 changes: 54 additions & 54 deletions rts/Sim/Misc/CollisionHandler.cpp
View file
Expand Up @@ -398,11 +398,11 @@ bool CCollisionHandler::IntersectEllipsoid(const CollisionVolume* v, const float
{
// transform the volume-space points into (unit) sphere-space (requires fewer
// float-ops than solving the surface equation for arbitrary ellipsoid volumes)
const float3 pii0 = pi0 * v->GetHIScales();
const float3 pii1 = pi1 * v->GetHIScales();
const float3 upi0 = pi0 * v->GetHIScales();
const float3 upi1 = pi1 * v->GetHIScales();
const float rSq = 1.0f;

if (pii0.dot(pii0) <= rSq) {
if (upi0.dot(upi0) <= rSq) {
if (q != nullptr) {
// terminate early in the special case
// that ray-segment originated *in* <v>
Expand All @@ -414,17 +414,17 @@ bool CCollisionHandler::IntersectEllipsoid(const CollisionVolume* v, const float
}

// get the ray direction in unit-sphere space
const float3 dir = (pii1 - pii0).SafeNormalize();
const float3 dir = (upi1 - upi0).SafeNormalize();

// solves [ x^2 + y^2 + z^2 == r^2 ] for t; closest
// point on ray is p(t) = p0 + (p1-p0)*t = p0 + d*t
// (A represents dir.dot(dir), which equals 1 since
// the ray direction is already normalized)
// const float A = (pii1 - pii0).dot(pii1 - pii0);
// const float B = 2.0f * pii0.dot(pii1 - pii0);
// const float A = (upi1 - upi0).dot(upi1 - upi0);
// const float B = 2.0f * upi0.dot(upi1 - upi0);
const float A = 1.0f;
const float B = 2.0f * pii0.dot(dir);
const float C = pii0.dot(pii0) - rSq;
const float B = 2.0f * upi0.dot(dir);
const float C = upi0.dot(upi0) - rSq;
const float D = (B * B) - (4.0f * A * C);

if (D < -COLLISION_VOLUME_EPS)
Expand All @@ -438,7 +438,7 @@ bool CCollisionHandler::IntersectEllipsoid(const CollisionVolume* v, const float
const float t0 = -B * 0.5f;
// const float t0 = -B / (2.0f * A);
// get the intersection point in sphere-space
const float3 pTmp = pii0 + (dir * t0);
const float3 pTmp = upi0 + (dir * t0);
// get the intersection point in volume-space
const float3 p0 = pTmp * v->GetHScales();
// get the distance from the start of the segment
Expand All @@ -464,8 +464,8 @@ bool CCollisionHandler::IntersectEllipsoid(const CollisionVolume* v, const float
// const float t0 = (-B + rD) / (2.0f * A);
// const float t1 = (-B - rD) / (2.0f * A);
// get the intersection points in sphere-space
const float3 pTmp0 = pii0 + (dir * t0);
const float3 pTmp1 = pii0 + (dir * t1);
const float3 pTmp0 = upi0 + (dir * t0);
const float3 pTmp1 = upi0 + (dir * t1);
// get the intersection points in volume-space
const float3 p0 = pTmp0 * v->GetHScales();
const float3 p1 = pTmp1 * v->GetHScales();
Expand Down Expand Up @@ -514,15 +514,15 @@ bool CCollisionHandler::IntersectCylinder(const CollisionVolume* v, const float3
const float3 dir = (pi1 - pi0).SafeNormalize();

// ray direction in (unit) cylinder-space
float3 diir = ZeroVector;
float3 udir;

// ray terminals in (unit) cylinder-space
float3 pii0 = pi0;
float3 pii1 = pi1;
float3 upi0 = pi0;
float3 upi1 = pi1;

// end-cap plane normals in volume-space
float3 n0 = ZeroVector;
float3 n1 = ZeroVector;
float3 n0;
float3 n1;

// (unit) cylinder-space to volume-space transformation
float3 inv = OnesVector;
Expand All @@ -534,64 +534,64 @@ bool CCollisionHandler::IntersectCylinder(const CollisionVolume* v, const float3

switch (pAx) {
case CollisionVolume::COLVOL_AXIS_X: {
pii0.y = pi0.y * ahis.y;
pii0.z = pi0.z * ahis.z;
pii1.y = pi1.y * ahis.y;
pii1.z = pi1.z * ahis.z;
upi0.y = pi0.y * ahis.y;
upi0.z = pi0.z * ahis.z;
upi1.y = pi1.y * ahis.y;
upi1.z = pi1.z * ahis.z;

inv.y = ahs.y;
inv.z = ahs.z;
diir = (pii1 - pii0).SafeNormalize();
udir = (upi1 - upi0).SafeNormalize();

n0.x = -1.0f; // left
n1.x = 1.0f; // right

// yz-surface equation params
a = (diir.y * diir.y) + (diir.z * diir.z);
b = ((pii0.y * diir.y) + (pii0.z * diir.z)) * 2.0f;
c = (pii0.y * pii0.y) + (pii0.z * pii0.z) - 1.0f;
a = (udir.y * udir.y) + (udir.z * udir.z);
b = ((upi0.y * udir.y) + (upi0.z * udir.z)) * 2.0f;
c = (upi0.y * upi0.y) + (upi0.z * upi0.z) - 1.0f;
} break;
case CollisionVolume::COLVOL_AXIS_Y: {
pii0.x = pi0.x * ahis.x;
pii0.z = pi0.z * ahis.z;
pii1.x = pi1.x * ahis.x;
pii1.z = pi1.z * ahis.z;
upi0.x = pi0.x * ahis.x;
upi0.z = pi0.z * ahis.z;
upi1.x = pi1.x * ahis.x;
upi1.z = pi1.z * ahis.z;

inv.x = ahs.x;
inv.z = ahs.z;
diir = (pii1 - pii0).SafeNormalize();
udir = (upi1 - upi0).SafeNormalize();

n0.y = 1.0f; // top
n1.y = -1.0f; // bottom

// xz-surface equation params
a = (diir.x * diir.x) + (diir.z * diir.z);
b = ((pii0.x * diir.x) + (pii0.z * diir.z)) * 2.0f;
c = (pii0.x * pii0.x) + (pii0.z * pii0.z) - 1.0f;
a = (udir.x * udir.x) + (udir.z * udir.z);
b = ((upi0.x * udir.x) + (upi0.z * udir.z)) * 2.0f;
c = (upi0.x * upi0.x) + (upi0.z * upi0.z) - 1.0f;
} break;
case CollisionVolume::COLVOL_AXIS_Z: {
pii0.x = pi0.x * ahis.x;
pii0.y = pi0.y * ahis.y;
pii1.x = pi1.x * ahis.x;
pii1.y = pi1.y * ahis.y;
upi0.x = pi0.x * ahis.x;
upi0.y = pi0.y * ahis.y;
upi1.x = pi1.x * ahis.x;
upi1.y = pi1.y * ahis.y;

inv.x = ahs.x;
inv.y = ahs.y;
diir = (pii1 - pii0).SafeNormalize();
udir = (upi1 - upi0).SafeNormalize();

n0.z = 1.0f; // front
n1.z = -1.0f; // back

// xy-surface equation params
a = (diir.x * diir.x) + (diir.y * diir.y);
b = ((pii0.x * diir.x) + (pii0.y * diir.y)) * 2.0f;
c = (pii0.x * pii0.x) + (pii0.y * pii0.y) - 1.0f;
a = (udir.x * udir.x) + (udir.y * udir.y);
b = ((upi0.x * udir.x) + (upi0.y * udir.y)) * 2.0f;
c = (upi0.x * upi0.x) + (upi0.y * upi0.y) - 1.0f;
} break;
}

// volume-space intersection points
float3 p0 = ZeroVector;
float3 p1 = ZeroVector;
float3 p0;
float3 p1;

int b0 = CQ_POINT_NO_INT;
int b1 = CQ_POINT_NO_INT;
Expand All @@ -611,15 +611,15 @@ bool CCollisionHandler::IntersectCylinder(const CollisionVolume* v, const float3
// quadratic eq.; one or two surface intersections
if (d < COLLISION_VOLUME_EPS) {
t0 = -b / (2.0f * a);
p0 = (pii0 + (diir * t0)) * inv;
p0 = (upi0 + (udir * t0)) * inv;
s0 = (p0 - pi0).SqLength();
b0 = (s0 < segLenSq && math::fabs(p0[pAx]) < ahs[pAx]) * CQ_POINT_ON_RAY;
} else {
rd = math::sqrt(d);
t0 = (-b - rd) / (2.0f * a);
t1 = (-b + rd) / (2.0f * a);
p0 = (pii0 + (diir * t0)) * inv;
p1 = (pii0 + (diir * t1)) * inv;
p0 = (upi0 + (udir * t0)) * inv;
p1 = (upi0 + (udir * t1)) * inv;
s0 = (p0 - pi0).SqLength();
s1 = (p1 - pi0).SqLength();
b0 = (s0 < segLenSq && math::fabs(p0[pAx]) < ahs[pAx]) * CQ_POINT_ON_RAY;
Expand All @@ -629,7 +629,7 @@ bool CCollisionHandler::IntersectCylinder(const CollisionVolume* v, const float3
if (b != 0.0f) {
// linear eq.; one surface intersection
t0 = -c / b;
p0 = (pii0 + (diir * t0)) * inv;
p0 = (upi0 + (udir * t0)) * inv;
s0 = (p0 - pi0).SqLength();
b0 = (s0 < segLenSq && math::fabs(p0[pAx]) < ahs[pAx]) * CQ_POINT_ON_RAY;
}
Expand All @@ -639,13 +639,13 @@ bool CCollisionHandler::IntersectCylinder(const CollisionVolume* v, const float3
if (b0 == CQ_POINT_NO_INT) {
// p0 does not lie on ray segment, or does not fall
// between cylinder end-caps: check if segment goes
// through front cap (plane)
// through front cap (plane) in unit-volume space
// NOTE: normal n0 and dir should not be orthogonal
dp = n0.dot(dir);
dp = n0.dot(udir);
rd = (dp != 0.0f)? 1.0f / dp: 0.01f;

t0 = -(n0.dot(pi0) - ahs[pAx]) * rd;
p0 = pi0 + (dir * t0);
t0 = -(n0.dot(upi0) - ahs[pAx]) * rd;
p0 = (upi0 + (udir * t0)) * inv;
s0 = (p0 - pi0).SqLength();
ra =
(((p0[sAx0] * p0[sAx0]) / ahsq[sAx0]) +
Expand All @@ -655,13 +655,13 @@ bool CCollisionHandler::IntersectCylinder(const CollisionVolume* v, const float3
if (b1 == CQ_POINT_NO_INT) {
// p1 does not lie on ray segment, or does not fall
// between cylinder end-caps: check if segment goes
// through rear cap (plane)
// through rear cap (plane) in unit-volume space
// NOTE: normal n1 and dir should not be orthogonal
dp = n1.dot(dir);
dp = n1.dot(udir);
rd = (dp != 0.0f)? 1.0f / dp: 0.01f;

t1 = -(n1.dot(pi0) - ahs[pAx]) * rd;
p1 = pi0 + (dir * t1);
t1 = -(n1.dot(upi0) - ahs[pAx]) * rd;
p1 = (upi0 + (udir * t1)) * inv;
s1 = (p1 - pi0).SqLength();
ra =
(((p1[sAx0] * p1[sAx0]) / ahsq[sAx0]) +
Expand Down

0 comments on commit 871cc9b

Please sign in to comment.