bool GJKCollisionDetector::detect(const Convex& convex1, const Transform2& transform1, const Convex& convex2,
const Transform2& transform2, Penetration& penetration)
{
if (convex1.getType() == ConvexType::CIRCLE && convex2.getType() == ConvexType::CIRCLE)
{
return CircleDetector::detect(
reinterpret_cast<const Circle&>(convex1),
transform1,
reinterpret_cast<const Circle&>(convex2),
transform2,
penetration);
}
vector<fvec2> simplex;
MinkowskiSum minkowskiSum(convex1, transform1, convex2, transform2);
fvec2 direction = calcInitialDirection(convex1, transform1, convex2, transform2);
if (detect(minkowskiSum, simplex, direction))
{
penetrationSolver->findPenetration(simplex, minkowskiSum, penetration);
return true;
}
return false;
}
double area(Convex a) {
double sum = 0;
a.push_back(a[0]);
for (int i = 0; i < a.size() - 1; i++) {
sum += cross(a[i], a[i+1]);
}
return sum / 2.0;
}
Convex convexIntersection(Convex v1, Convex v2) {
vector<Halfplane> h;
for (int i = 0; i < v1.size(); i++) {
h.push_back(Halfplane(v1[i], v1[(i+1) % v1.size()]));
}
for (int i = 0; i < v2.size(); i++) {
h.push_back(Halfplane(v2[i], v2[(i+1) % v2.size()]));
}
return halfplaneIntersection(h);
}
fvec2 GJKCollisionDetector::calcInitialDirection(
const Convex& convex1,
const Transform2& transform1,
const Convex& convex2,
const Transform2& transform2)
{
fvec2 c1 = transform1.getTransformed(convex1.getCenter());
fvec2 c2 = transform2.getTransformed(convex2.getCenter());
return c2 - c1;
}
void input() {
for (int i = 0; i < n; i++) {
double x, y;
scanf("%lf%lf", &x, &y);
c1.push_back(Point(x, y));
}
for (int i = 0; i < m; i++) {
double x, y;
scanf("%lf%lf", &x, &y);
c2.push_back(Point(x, y));
}
}
void Convex::renderWorkingList()
{
//bool rendered = false;
CollisionWorkingList& rList = getWorkingList();
CollisionWorkingList* pList = rList.wLink.mNext;
while (pList != &rList) {
Convex* pConvex = pList->mConvex;
pConvex->render();
//rendered = true;
pList = pList->wLink.mNext;
}
//Log::warnf( "convex rendered - %s", rendered ? "YES" : "NO" );
}
void computeBV<AABB>(const Convex& s, AABB& bv)
{
Vec3f R[3];
matMulMat(s.getRotation(), s.getLocalRotation(), R);
Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
AABB bv_;
for(int i = 0; i < s.num_points; ++i)
{
Vec3f new_p = matMulVec(R, s.points[i]) + T;
bv_ += new_p;
}
bv = bv_;
}
int main()
{
Convex CX;
PointList AllPoints, HullPoints;
FacetList HullFacets;
CX.InputPointList(&AllPoints);
CX.Calculation (&HullFacets, &AllPoints );
CX.FacetToPoints(&HullFacets, &HullPoints);
CX.OutputPointList (&HullPoints);
return 0;
}
bool find_prim(const Complex& a, const Convex& b,
const Transform& a2w, const Transform& b2w,
Vector& v, ShapePtr& p) {
Transform b2a;
b2a.multInverseLeft(a2w, b2w);
BBox bb = b.bbox(b2a);
return find_prim(a.root, b, bb, b2a, v, p);
}
bool intersect(const Complex& a, const Convex& b,
const Transform& a2w, const Transform& b2w,
Vector& v) {
Transform b2a;
b2a.multInverseLeft(a2w, b2w);
BBox bb = b.bbox(b2a);
return intersect(a.root, b, bb, b2a, v);
}
bool common_point(const Complex& a, const Convex& b,
const Transform& a2w, const Transform& b2w,
Vector& v, Point& pa, Point& pb) {
Transform b2a;
b2a.multInverseLeft(a2w, b2w);
BBox bb = b.bbox(b2a);
return common_point(a.root, b, bb, b2a, v, pb, pa);
}
void computeBV<OBB>(const Convex& s, OBB& bv)
{
Vec3f R[3];
matMulMat(s.getRotation(), s.getLocalRotation(), R);
Vec3f T = matMulVec(s.getRotation(), s.getLocalTranslation()) + s.getTranslation();
fit(s.points, s.num_points, bv);
Vec3f axis[3];
axis[0] = matMulVec(R, bv.axis[0]);
axis[1] = matMulVec(R, bv.axis[1]);
axis[2] = matMulVec(R, bv.axis[2]);
bv.axis[0] = axis[0];
bv.axis[1] = axis[1];
bv.axis[2] = axis[2];
bv.To = matMulVec(R, bv.To) + T;
}
bool GJKCollisionDetector::detect(
const Convex& convex1,
const Transform2& transform1,
const Convex& convex2,
const Transform2& transform2)
{
if (convex1.getType() == ConvexType::CIRCLE && convex2.getType() == ConvexType::CIRCLE)
{
return CircleDetector::detect(
reinterpret_cast<const Circle&>(convex1),
transform1,
reinterpret_cast<const Circle&>(convex2),
transform2);
}
vector<fvec2> simplex;
MinkowskiSum minkowskiSum(convex1, transform1, convex2, transform2);
fvec2 direction = calcInitialDirection(convex1, transform1, convex2, transform2);
return detect(minkowskiSum, simplex, direction);
}
void Convex::updateStateList(const MatrixF& mat, const Point3F& scale, const Point3F* displacement)
{
PROFILE_SCOPE( Convex_UpdateStateList );
Box3F box1 = getBoundingBox(mat, scale);
box1.minExtents -= Point3F(1, 1, 1);
box1.maxExtents += Point3F(1, 1, 1);
if (displacement) {
Point3F oldMin = box1.minExtents;
Point3F oldMax = box1.maxExtents;
box1.minExtents.setMin(oldMin + *displacement);
box1.minExtents.setMin(oldMax + *displacement);
box1.maxExtents.setMax(oldMin + *displacement);
box1.maxExtents.setMax(oldMax + *displacement);
}
sTag++;
// Destroy states which are no longer intersecting
for (CollisionStateList* itr = mList.mNext; itr != &mList; itr = itr->mNext) {
Convex* cv = (itr->mState->a == this)? itr->mState->b: itr->mState->a;
cv->mTag = sTag;
if (!box1.isOverlapped(cv->getBoundingBox())) {
CollisionState* cs = itr->mState;
itr = itr->mPrev;
delete cs;
}
}
// Add collision states for new overlapping objects
for (CollisionWorkingList* itr0 = mWorking.wLink.mNext; itr0 != &mWorking; itr0 = itr0->wLink.mNext) {
register Convex* cv = itr0->mConvex;
if (cv->mTag != sTag && box1.isOverlapped(cv->getBoundingBox())) {
CollisionState* state = new GjkCollisionState;
state->set(this,cv,mat,cv->getTransform());
state->mLista->linkAfter(&mList);
state->mListb->linkAfter(&cv->mList);
}
}
}
bool Convex_Intersects_Polygon(const Convex &c, const Polygon &p)
{
LineSegment l;
l.a = p.p.back();
for(size_t i = 0; i < p.p.size(); ++i)
{
l.b = p.p[i];
if (c.Intersects(l))
return true;
l.a = l.b;
}
// Check all the edges of the convex shape against the polygon.
for(int i = 0; i < c.NumEdges(); ++i)
{
l = c.Edge(i);
if (p.Intersects(l))
return true;
}
return false;
}
int main(int argc, char const *argv[])
{
Convex v1, v2;
v1.push_back(Point(0,0));
v1.push_back(Point(1,1));
v1.push_back(Point(0,2));
v1.push_back(Point(-1,1));
v1.push_back(Point(-0.5,0));
v2.push_back(Point(1,0));
v2.push_back(Point(2,1));
v2.push_back(Point(1,2));
v2.push_back(Point(0,1));
v2.push_back(Point(0.5,0));
Convex v = convexIntersection(v1, v2);
for (int i = 0; i < v.size(); ++i)
{
cout << v[i] << endl;
}
return 0;
}
Convex convex_hull(Convex a) {
Convex res(2 * a.size() + 5);
sort(a.begin(), a.end(), comp_less);
a.erase(unique(a.begin(), a.end()), a.end());
int m = 0;
for (int i = 0; i < a.size(); i++) {
while (m > 1 && sgn(cross(res[m-1]-res[m-2], a[i]-res[m-2])) <= 0) {
m--;
}
res[m++] = a[i];
}
int k = m;
for (int i = a.size() - 2; i >= 0; i--) {
while (m > k && sgn(cross(res[m-1]-res[m-2], a[i]-res[m-2])) <= 0) {
m--;
}
res[m++] = a[i];
}
res.resize(m);
if (a.size() > 1) {
res.resize(m-1);
}
return res;
}
bool Intersects(const Convex& other, Point& overlap) const override
{
return other.Intersects(*this, overlap);
}
Point3F Etherform::_move( const F32 travelTime, Collision *outCol )
{
// Try and move to new pos
F32 totalMotion = 0.0f;
// TODO: not used?
//F32 initialSpeed = mVelocity.len();
Point3F start;
Point3F initialPosition;
getTransform().getColumn(3,&start);
initialPosition = start;
static CollisionList collisionList;
static CollisionList physZoneCollisionList;
collisionList.clear();
physZoneCollisionList.clear();
MatrixF collisionMatrix(true);
collisionMatrix.setColumn(3, start);
VectorF firstNormal(0.0f, 0.0f, 0.0f);
F32 time = travelTime;
U32 count = 0;
static Polyhedron sBoxPolyhedron;
static ExtrudedPolyList sExtrudedPolyList;
static ExtrudedPolyList sPhysZonePolyList;
for (; count < sMoveRetryCount; count++) {
F32 speed = mVelocity.len();
if(!speed)
break;
Point3F end = start + mVelocity * time;
Point3F distance = end - start;
if (mFabs(distance.x) < mObjBox.len_x() &&
mFabs(distance.y) < mObjBox.len_y() &&
mFabs(distance.z) < mObjBox.len_z())
{
// We can potentially early out of this. If there are no polys in the clipped polylist at our
// end position, then we can bail, and just set start = end;
Box3F wBox = mScaledBox;
wBox.minExtents += end;
wBox.maxExtents += end;
static EarlyOutPolyList eaPolyList;
eaPolyList.clear();
eaPolyList.mNormal.set(0.0f, 0.0f, 0.0f);
eaPolyList.mPlaneList.clear();
eaPolyList.mPlaneList.setSize(6);
eaPolyList.mPlaneList[0].set(wBox.minExtents,VectorF(-1.0f, 0.0f, 0.0f));
eaPolyList.mPlaneList[1].set(wBox.maxExtents,VectorF(0.0f, 1.0f, 0.0f));
eaPolyList.mPlaneList[2].set(wBox.maxExtents,VectorF(1.0f, 0.0f, 0.0f));
eaPolyList.mPlaneList[3].set(wBox.minExtents,VectorF(0.0f, -1.0f, 0.0f));
eaPolyList.mPlaneList[4].set(wBox.minExtents,VectorF(0.0f, 0.0f, -1.0f));
eaPolyList.mPlaneList[5].set(wBox.maxExtents,VectorF(0.0f, 0.0f, 1.0f));
// Build list from convex states here...
CollisionWorkingList& rList = mConvex.getWorkingList();
CollisionWorkingList* pList = rList.wLink.mNext;
while (pList != &rList) {
Convex* pConvex = pList->mConvex;
if (pConvex->getObject()->getTypeMask() & sCollisionMoveMask) {
Box3F convexBox = pConvex->getBoundingBox();
if (wBox.isOverlapped(convexBox))
{
// No need to separate out the physical zones here, we want those
// to cause a fallthrough as well...
pConvex->getPolyList(&eaPolyList);
}
}
pList = pList->wLink.mNext;
}
if (eaPolyList.isEmpty())
{
totalMotion += (end - start).len();
start = end;
break;
}
}
collisionMatrix.setColumn(3, start);
sBoxPolyhedron.buildBox(collisionMatrix, mScaledBox, true);
// Setup the bounding box for the extrudedPolyList
Box3F plistBox = mScaledBox;
collisionMatrix.mul(plistBox);
Point3F oldMin = plistBox.minExtents;
Point3F oldMax = plistBox.maxExtents;
plistBox.minExtents.setMin(oldMin + (mVelocity * time) - Point3F(0.1f, 0.1f, 0.1f));
plistBox.maxExtents.setMax(oldMax + (mVelocity * time) + Point3F(0.1f, 0.1f, 0.1f));
// Build extruded polyList...
VectorF vector = end - start;
sExtrudedPolyList.extrude(sBoxPolyhedron,vector);
sExtrudedPolyList.setVelocity(mVelocity);
sExtrudedPolyList.setCollisionList(&collisionList);
sPhysZonePolyList.extrude(sBoxPolyhedron,vector);
sPhysZonePolyList.setVelocity(mVelocity);
sPhysZonePolyList.setCollisionList(&physZoneCollisionList);
// Build list from convex states here...
CollisionWorkingList& rList = mConvex.getWorkingList();
CollisionWorkingList* pList = rList.wLink.mNext;
while (pList != &rList) {
Convex* pConvex = pList->mConvex;
if (pConvex->getObject()->getTypeMask() & sCollisionMoveMask) {
Box3F convexBox = pConvex->getBoundingBox();
if (plistBox.isOverlapped(convexBox))
{
if (pConvex->getObject()->getTypeMask() & PhysicalZoneObjectType)
pConvex->getPolyList(&sPhysZonePolyList);
else
pConvex->getPolyList(&sExtrudedPolyList);
}
}
pList = pList->wLink.mNext;
}
// Take into account any physical zones...
for (U32 j = 0; j < physZoneCollisionList.getCount(); j++)
{
AssertFatal(dynamic_cast<PhysicalZone*>(physZoneCollisionList[j].object), "Bad phys zone!");
const PhysicalZone* pZone = (PhysicalZone*)physZoneCollisionList[j].object;
if (pZone->isActive())
mVelocity *= pZone->getVelocityMod();
}
if (collisionList.getCount() != 0 && collisionList.getTime() < 1.0f)
{
// Set to collision point
F32 velLen = mVelocity.len();
F32 dt = time * getMin(collisionList.getTime(), 1.0f);
start += mVelocity * dt;
time -= dt;
totalMotion += velLen * dt;
// Back off...
if ( velLen > 0.f ) {
F32 newT = getMin(0.01f / velLen, dt);
start -= mVelocity * newT;
totalMotion -= velLen * newT;
}
// Pick the surface most parallel to the face that was hit.
const Collision *collision = &collisionList[0];
const Collision *cp = collision + 1;
const Collision *ep = collision + collisionList.getCount();
for (; cp != ep; cp++)
{
if (cp->faceDot > collision->faceDot)
collision = cp;
}
F32 bd = -mDot(mVelocity, collision->normal);
// Copy this collision out so
// we can use it to do impacts
// and query collision.
*outCol = *collision;
// Subtract out velocity
VectorF dv = collision->normal * (bd + sNormalElasticity);
mVelocity += dv;
if (count == 0)
{
firstNormal = collision->normal;
}
else
{
if (count == 1)
{
// Re-orient velocity along the crease.
if (mDot(dv,firstNormal) < 0.0f &&
mDot(collision->normal,firstNormal) < 0.0f)
{
VectorF nv;
mCross(collision->normal,firstNormal,&nv);
F32 nvl = nv.len();
if (nvl)
{
if (mDot(nv,mVelocity) < 0.0f)
nvl = -nvl;
nv *= mVelocity.len() / nvl;
mVelocity = nv;
}
}
}
}
}
else
{
totalMotion += (end - start).len();
start = end;
break;
}
}
if (count == sMoveRetryCount)
{
// Failed to move
start = initialPosition;
mVelocity.set(0.0f, 0.0f, 0.0f);
}
return start;
}
void Etherform::_findContact( SceneObject **contactObject,
VectorF *contactNormal,
Vector<SceneObject*> *outOverlapObjects )
{
Point3F pos;
getTransform().getColumn(3,&pos);
Box3F wBox;
Point3F exp(0,0,sTractionDistance);
wBox.minExtents = pos + mScaledBox.minExtents - exp;
wBox.maxExtents.x = pos.x + mScaledBox.maxExtents.x;
wBox.maxExtents.y = pos.y + mScaledBox.maxExtents.y;
wBox.maxExtents.z = pos.z + mScaledBox.minExtents.z + sTractionDistance;
static ClippedPolyList polyList;
polyList.clear();
polyList.doConstruct();
polyList.mNormal.set(0.0f, 0.0f, 0.0f);
polyList.setInterestNormal(Point3F(0.0f, 0.0f, -1.0f));
polyList.mPlaneList.setSize(6);
polyList.mPlaneList[0].setYZ(wBox.minExtents, -1.0f);
polyList.mPlaneList[1].setXZ(wBox.maxExtents, 1.0f);
polyList.mPlaneList[2].setYZ(wBox.maxExtents, 1.0f);
polyList.mPlaneList[3].setXZ(wBox.minExtents, -1.0f);
polyList.mPlaneList[4].setXY(wBox.minExtents, -1.0f);
polyList.mPlaneList[5].setXY(wBox.maxExtents, 1.0f);
Box3F plistBox = wBox;
// Expand build box as it will be used to collide with items.
// PickupRadius will be at least the size of the box.
F32 pd = 0;
wBox.minExtents.x -= pd; wBox.minExtents.y -= pd;
wBox.maxExtents.x += pd; wBox.maxExtents.y += pd;
wBox.maxExtents.z = pos.z + mScaledBox.maxExtents.z;
// Build list from convex states here...
CollisionWorkingList& rList = mConvex.getWorkingList();
CollisionWorkingList* pList = rList.wLink.mNext;
while (pList != &rList)
{
Convex* pConvex = pList->mConvex;
U32 objectMask = pConvex->getObject()->getTypeMask();
if ( ( objectMask & sCollisionMoveMask ) &&
!( objectMask & PhysicalZoneObjectType ) )
{
Box3F convexBox = pConvex->getBoundingBox();
if (plistBox.isOverlapped(convexBox))
pConvex->getPolyList(&polyList);
}
else
outOverlapObjects->push_back( pConvex->getObject() );
pList = pList->wLink.mNext;
}
if (!polyList.isEmpty())
{
// Pick flattest surface
F32 bestVd = -1.0f;
ClippedPolyList::Poly* poly = polyList.mPolyList.begin();
ClippedPolyList::Poly* end = polyList.mPolyList.end();
for (; poly != end; poly++)
{
F32 vd = poly->plane.z; // i.e. mDot(Point3F(0,0,1), poly->plane);
if (vd > bestVd)
{
bestVd = vd;
*contactObject = poly->object;
*contactNormal = poly->plane;
}
}
}
}
int main()
{
try
{
Convex c;
c.add(Point(0,0));
c.show();
c.add(Point(1,0));
c.show();
c.add(Point(0,1));
c.show();
c.add(Point(1,1));
c.add(Point(0.5,0.5));
c.show();
c.add(Point(2,2));
c.add(Point(2,0));
c.add(Point(0,2));
c.show();
c.add(Point(1,5));
c.add(Point(1,1.4));
c.add(Point(0.2,0.2));
cout<<"Show smt"<<endl;
c.show();
Convex cc;
cc.add(Point(0,0));
cc.add(Point(0,0));
cc.add(Point(0,0));
}
catch(const Polygon::BadInit& bd)
{
bd.diagnos();
}
return 0;
}
void init() {
c1.clear();
c2.clear();
}
//! 行列変換した後の最小座標，最大座標計算
Vec3x2 calcMinMax(const AMat43& m) const {
Convex c = *this * m;
return c.calcMinMax();
}
Convex convexIntersection(Convex v1, Convex v2)
{
vector<Halfplane> h, h1, h2;
for (int i = 0; i < v1.size(); ++i)
h1.push_back(Halfplane(v1[i], v1[(i+1)%v1.size()]));
for (int i = 0; i < v2.size(); ++i)
h2.push_back(Halfplane(v2[i], v2[(i+1)%v2.size()]));
int p1 = 0, p2 = 0;
while(p1 < h1.size() && p2 < h2.size())
{
int res = sgn(arg(h1[p1].second - h1[p1].first) - arg(h2[p2].second - h2[p2].first));
if (res < 0)
h.push_back(h1[p1++]);
else if (res > 0)
h.push_back(h2[p2++]);
else
if (sgn(cross(h1[p1].first - h2[p2].first, h2[p2].second - h2[p2].first)) < 0)
{
h.push_back(h1[p1++]);
p2++;
}
else
{
h.push_back(h2[p2++]);
p1++;
}
}
while(p1 < h1.size())
h.push_back(h1[p1++]);
while(p2 < h2.size())
h.push_back(h2[p2++]);
deque<Halfplane> q;
deque<Point> ans;
q.push_back(h[0]);
for (int i = 1; i < int(h.size()); ++i)
{
if (sgn(arg(h[i].second - h[i].first) - arg(h[i-1].second - h[i-1].first)) == 0)
continue;
while (ans.size() > 0 && !satisfy(ans.back(), h[i]))
{
ans.pop_back();
q.pop_back();
}
while (ans.size() > 0 && !satisfy(ans.front(), h[i]))
{
ans.pop_front();
q.pop_front();
}
ans.push_back(crosspoint(q.back(), h[i]));
q.push_back(h[i]);
}
while (ans.size() > 0 && !satisfy(ans.back(), q.front()))
{
ans.pop_back();
q.pop_back();
}
while (ans.size() > 0 && !satisfy(ans.front(), q.back()))
{
ans.pop_front();
q.pop_front();
}
ans.push_back(crosspoint(q.back(), q.front()));
return vector<Point>(ans.begin(), ans.end());
}
void TerrainBlock::buildConvex(const Box3F& box,Convex* convex)
{
sTerrainConvexList.collectGarbage();
//
if (box.maxExtents.z < -TerrainThickness || box.minExtents.z > fixedToFloat(gridMap[BlockShift]->maxHeight))
return;
// Transform the bounding sphere into the object's coord space. Note that this
// not really optimal.
Box3F osBox = box;
mWorldToObj.mul(osBox);
AssertWarn(mObjScale == Point3F(1, 1, 1), "Error, handle the scale transform on the terrain");
S32 xStart = (S32)mFloor( osBox.minExtents.x / mSquareSize );
S32 xEnd = (S32)mCeil ( osBox.maxExtents.x / mSquareSize );
S32 yStart = (S32)mFloor( osBox.minExtents.y / mSquareSize );
S32 yEnd = (S32)mCeil ( osBox.maxExtents.y / mSquareSize );
S32 xExt = xEnd - xStart;
if (xExt > MaxExtent)
xExt = MaxExtent;
mHeightMax = floatToFixed(osBox.maxExtents.z);
mHeightMin = (osBox.minExtents.z < 0)? 0: floatToFixed(osBox.minExtents.z);
for (S32 y = yStart; y < yEnd; y++) {
S32 yi = y & BlockMask;
//
for (S32 x = xStart; x < xEnd; x++) {
S32 xi = x & BlockMask;
GridSquare *gs = findSquare(0, Point2I(xi, yi));
// If we disable repeat, then skip non-primary
if(!mTile && (x!=xi || y!=yi))
continue;
// holes only in the primary terrain block
if (((gs->flags & GridSquare::Empty) && x == xi && y == yi) ||
gs->minHeight > mHeightMax || gs->maxHeight < mHeightMin)
continue;
U32 sid = (x << 16) + (y & ((1 << 16) - 1));
Convex* cc = 0;
// See if the square already exists as part of the working set.
CollisionWorkingList& wl = convex->getWorkingList();
for (CollisionWorkingList* itr = wl.wLink.mNext; itr != &wl; itr = itr->wLink.mNext)
if (itr->mConvex->getType() == TerrainConvexType &&
static_cast<TerrainConvex*>(itr->mConvex)->squareId == sid) {
cc = itr->mConvex;
break;
}
if (cc)
continue;
// Create a new convex.
TerrainConvex* cp = new TerrainConvex;
sTerrainConvexList.registerObject(cp);
convex->addToWorkingList(cp);
cp->halfA = true;
cp->square = 0;
cp->mObject = this;
cp->squareId = sid;
cp->material = getMaterial(xi,yi)->index;//RDTODO
cp->box.minExtents.set((F32)(x * mSquareSize), (F32)(y * mSquareSize), fixedToFloat(gs->minHeight));
cp->box.maxExtents.x = cp->box.minExtents.x + mSquareSize;
cp->box.maxExtents.y = cp->box.minExtents.y + mSquareSize;
cp->box.maxExtents.z = fixedToFloat(gs->maxHeight);
mObjToWorld.mul(cp->box);
// Build points
Point3F* pos = cp->point;
for (int i = 0; i < 4 ; i++,pos++) {
S32 dx = i >> 1;
S32 dy = dx ^ (i & 1);
pos->x = (F32)((x + dx) * mSquareSize);
pos->y = (F32)((y + dy) * mSquareSize);
pos->z = fixedToFloat(getHeight(xi + dx, yi + dy));
}
// Build normals, then split into two Convex objects if the
// square is concave
if ((cp->split45 = gs->flags & GridSquare::Split45) == true) {
VectorF *vp = cp->point;
mCross(vp[0] - vp[1],vp[2] - vp[1],&cp->normal[0]);
cp->normal[0].normalize();
mCross(vp[2] - vp[3],vp[0] - vp[3],&cp->normal[1]);
cp->normal[1].normalize();
if (mDot(vp[3] - vp[1],cp->normal[0]) > 0) {
TerrainConvex* nc = new TerrainConvex(*cp);
sTerrainConvexList.registerObject(nc);
convex->addToWorkingList(nc);
nc->halfA = false;
nc->square = cp;
cp->square = nc;
}
}
else {
VectorF *vp = cp->point;
mCross(vp[3] - vp[0],vp[1] - vp[0],&cp->normal[0]);
cp->normal[0].normalize();
mCross(vp[1] - vp[2],vp[3] - vp[2],&cp->normal[1]);
cp->normal[1].normalize();
if (mDot(vp[2] - vp[0],cp->normal[0]) > 0) {
TerrainConvex* nc = new TerrainConvex(*cp);
sTerrainConvexList.registerObject(nc);
convex->addToWorkingList(nc);
nc->halfA = false;
nc->square = cp;
cp->square = nc;
}
}
}
}
}
bool Intersects(const Convex& other, Point& intersection) const override
{
return other.Intersects(*this, intersection);
}
bool Intersects(const Convex& other) const override
{
return other.Intersects(*this);
}
//-----------------------------------------------------------------------------
//
// VActorPhysicsController::findCollision( pCollision );
//
// ...
//
//-----------------------------------------------------------------------------
bool VActorPhysicsController::findCollision( Collision *&pCollision )
{
// Setup Collision List.
static CollisionList sCollisionList;
sCollisionList.clear();
static Polyhedron sBoxPolyhedron;
static ExtrudedPolyList sExtrudedPolyList;
// Determine Positions.
const Point3F preTickPosition = getPosition();
const VectorF preTickVelocity = getVelocity();
const Point3F postTickPosition = preTickPosition + ( preTickVelocity * TickSec );
const VectorF postTickVector = postTickPosition - preTickPosition;
// Construct Scaled Box.
Box3F scaledBox = mObject->getObjBox();
scaledBox.minExtents.convolve( mObject->getScale() );
scaledBox.maxExtents.convolve( mObject->getScale() );
// Setup Polyherdron.
MatrixF collisionMatrix( true );
collisionMatrix.setPosition( preTickPosition );
sBoxPolyhedron.buildBox( collisionMatrix, scaledBox );
// Setup Extruded Poly List.
sExtrudedPolyList.extrude( sBoxPolyhedron, postTickVector );
sExtrudedPolyList.setVelocity( preTickVelocity );
sExtrudedPolyList.setCollisionList( &sCollisionList );
// Construct World Convex Box & Adjust for Sweep.
Box3F convexBox = scaledBox;
getTransform().mul( convexBox );
convexBox.minExtents += postTickVector;
convexBox.maxExtents += postTickVector;
// Determine the Collision Mask.
const U32 collisionMask = ( isInWater() ) ? ( sGroundCollisionMask | sMoveCollisionMask ) : sMoveCollisionMask;
// Build List of Contacts.
CollisionWorkingList &rList = mConvex.getWorkingList();
for ( CollisionWorkingList *pList = rList.wLink.mNext; pList != &rList; pList = pList->wLink.mNext )
{
Convex *convexShape = pList->mConvex;
// Valid Collision Target?
if ( !( convexShape->getObject()->getTypeMask() & collisionMask ) )
{
// No, Continue.
continue;
}
// Overlap?
const Box3F &collisionConvexBox = convexShape->getBoundingBox();
if ( convexBox.isOverlapped( collisionConvexBox ) )
{
// Build Contact Information.
convexShape->getPolyList( &sExtrudedPolyList );
}
}
// Valid Collision?
if ( sCollisionList.getCount() == 0 || sCollisionList.getTime() > 1.f )
{
// No, Quit Now.
return false;
}
// Use First Collision.
Collision *collision = &sCollisionList[0];
// More Collisions?
if ( sCollisionList.getCount() > 1 )
{
// Check for Better Contacts.
for ( Collision *cp = ( collision + 1 ); cp != ( collision + sCollisionList.getCount() ); cp++ )
{
if ( cp->faceDot > collision->faceDot )
{
// Use this One.
collision = cp;
}
}
}
// Store Reference.
pCollision = collision;
// Valid Collision.
return true;
}
//-----------------------------------------------------------------------------
//
// VActorPhysicsController::findGroundContact( pContactObject, pContactPoint, pContactNormal );
//
// ...
//
//-----------------------------------------------------------------------------
bool VActorPhysicsController::findGroundContact( SceneObject *&pContactObject, Point3F &pContactPoint, VectorF &pContactNormal )
{
// Setup Collision List.
static CollisionList sCollisionList;
sCollisionList.clear();
static Polyhedron sBoxPolyhedron;
static ExtrudedPolyList sExtrudedPolyList;
// Fetch Max Step Height.
const F32 stepHeight = mObject->getDataBlock()->getMaxStepHeight();
// Determine Positions.
const Point3F preTickPosition = getPosition() + Point3F( 0.f, 0.f, stepHeight );
const VectorF preTickVelocity = getVelocity() + mGravity - VectorF( 0.f, 0.f, stepHeight / TickSec );
const Point3F postTickPosition = preTickPosition + ( preTickVelocity * TickSec );
const VectorF postTickVector = postTickPosition - preTickPosition;
// Construct Scaled Box.
Box3F scaledBox = mObject->getObjBox();
scaledBox.minExtents.convolve( mObject->getScale() );
scaledBox.maxExtents.convolve( mObject->getScale() );
// Setup Polyherdron.
MatrixF collisionMatrix( true );
collisionMatrix.setPosition( preTickPosition );
sBoxPolyhedron.buildBox( collisionMatrix, scaledBox, true );
// Setup Extruded Poly List.
sExtrudedPolyList.extrude( sBoxPolyhedron, postTickVector );
sExtrudedPolyList.setVelocity( preTickVelocity );
sExtrudedPolyList.setCollisionList( &sCollisionList );
// Construct World Convex Box & Adjust for Sweep.
Box3F convexBox = scaledBox;
getTransform().mul( convexBox );
convexBox.minExtents += postTickVector;
convexBox.maxExtents += postTickVector;
// Build List of Contacts.
CollisionWorkingList &rList = mConvex.getWorkingList();
for ( CollisionWorkingList *pList = rList.wLink.mNext; pList != &rList; pList = pList->wLink.mNext )
{
Convex *convexShape = pList->mConvex;
// Ground Object?
if ( !( convexShape->getObject()->getTypeMask() & sGroundCollisionMask ) )
{
// No, Continue.
continue;
}
// Overlap?
const Box3F &collisionConvexBox = convexShape->getBoundingBox();
if ( convexBox.isOverlapped( collisionConvexBox ) )
{
// Build Contact Information.
convexShape->getPolyList( &sExtrudedPolyList );
}
}
// Valid Collision?
if ( sCollisionList.getCount() == 0 || sCollisionList.getTime() < 0.f || sCollisionList.getTime() > 1.f )
{
// No, Quit Now.
return false;
}
// Use First Collision.
Collision *collision = &sCollisionList[0];
// More Collisions?
if ( sCollisionList.getCount() > 1 )
{
// Check for Better Contacts.
for ( Collision *cp = ( collision + 1 ); cp != ( collision + sCollisionList.getCount() ); cp++ )
{
if ( cp->faceDot > collision->faceDot )
{
// Use this One.
collision = cp;
}
}
}
// Set Properties.
pContactObject = collision->object;
//pContactPoint = collision->point;
pContactPoint = ( preTickPosition + ( preTickVelocity * TickSec * sCollisionList.getTime() ) );
pContactNormal = collision->normal;
// Valid Contact.
return true;
}
