bool SteerLib::GJK_EPA::GJK(std::vector<Util::Vector>& _simplex, const std::vector<Util::Vector>& _shapeA, const std::vector<Util::Vector>& _shapeB)
{
// Get random starting direction
Util::Vector d(1, 0, 0);
// Get first point for minkowski difference
// Then add it to the Simplex
Util::Vector A = Support(_shapeA, _shapeB, d);
_simplex.push_back(A);
// Negate d for next point
d = -d;
while (true) {
// Get next point for simplex
// Then add it to the Simplex
Util::Vector B = Support(_shapeA, _shapeB, d);
_simplex.push_back(B);
// Make sure B passes origin via dot product
float dotproduct = DotProduct(B, d);
if (dotproduct <= 0) {
// Didn't pass origin so it probably won't ever
return false;
}
else {
// You made it this far, so check the origin
if (SimplexOrigins(_simplex, d)) {
// Collides!
return true;
}
}
}
return false;
}
sBool sDiskItem::SetBinary(sInt attr,sU8 *buffer,sInt size)
{
if(sDiskItemTypes[attr][0]==sDIT_BINARY && Support(attr))
return SetAttr(attr,buffer,size);
else
return sFALSE;
}
void SteerLib::GJK_EPA::EPA(float& return_penetration_depth, Util::Vector& return_penetration_vector, const std::vector<Util::Vector>& _simplex, const std::vector<Util::Vector>& _shapeA, const std::vector<Util::Vector>& _shapeB)
{
std::vector<Util::Vector> simplex = _simplex; // Copy the original so we can expand it.
Util::Vector normal;
Edge closestEdge;
float epsilon = 0.0001; // Should be a small number.
while (true)
{
closestEdge = findClosestEdge(simplex);
Util::Vector supportVector = Support(_shapeA, _shapeB, closestEdge.normal);
float d = DotProduct(supportVector, closestEdge.normal);
if (d - closestEdge.distance < epsilon)
{
return_penetration_vector = closestEdge.normal;
return_penetration_depth = d;
return;
}
else
simplex.insert(simplex.begin() + closestEdge.index, supportVector);
}
}
// Recursive implementation of the EPA loop.
// Each recursion adds a point to the convex hull until it's known that we have the closest point on the surface.
static struct ClosestPoints
EPARecurse(const struct SupportContext *ctx, const int count, const struct MinkowskiPoint *hull, const int iteration)
{
int mini = 0;
cpFloat minDist = INFINITY;
// TODO: precalculate this when building the hull and save a step.
// Find the closest segment hull[i] and hull[i + 1] to (0, 0)
for(int j=0, i=count-1; j<count; i=j, j++) {
cpFloat d = ClosestDist(hull[i].ab, hull[j].ab);
if(d < minDist) {
minDist = d;
mini = i;
}
}
struct MinkowskiPoint v0 = hull[mini];
struct MinkowskiPoint v1 = hull[(mini + 1)%count];
cpAssertSoft(!cpveql(v0.ab, v1.ab), "Internal Error: EPA vertexes are the same (%d and %d)", mini, (mini + 1)%count);
// Check if there is a point on the minkowski difference beyond this edge.
struct MinkowskiPoint p = Support(ctx, cpvperp(cpvsub(v1.ab, v0.ab)));
#if DRAW_EPA
cpVect verts[count];
for(int i=0; i<count; i++) verts[i] = hull[i].ab;
ChipmunkDebugDrawPolygon(count, verts, 0.0, RGBAColor(1, 1, 0, 1), RGBAColor(1, 1, 0, 0.25));
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5, p.ab, LAColor(1, 1));
#endif
if(CheckArea(cpvsub(v1.ab, v0.ab), cpvadd(cpvsub(p.ab, v0.ab), cpvsub(p.ab, v1.ab))) && iteration < MAX_EPA_ITERATIONS) {
// Rebuild the convex hull by inserting p.
struct MinkowskiPoint *hull2 = (struct MinkowskiPoint *)alloca((count + 1)*sizeof(struct MinkowskiPoint));
int count2 = 1;
hull2[0] = p;
for(int i=0; i<count; i++) {
int index = (mini + 1 + i)%count;
cpVect h0 = hull2[count2 - 1].ab;
cpVect h1 = hull[index].ab;
cpVect h2 = (i + 1 < count ? hull[(index + 1)%count] : p).ab;
if(CheckArea(cpvsub(h2, h0), cpvadd(cpvsub(h1, h0), cpvsub(h1, h2)))) {
hull2[count2] = hull[index];
count2++;
}
}
return EPARecurse(ctx, count2, hull2, iteration + 1);
} else {
// Could not find a new point to insert, so we have found the closest edge of the minkowski difference.
cpAssertWarn(iteration < WARN_EPA_ITERATIONS, "High EPA iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
}
sInt sDiskItem::GetBinarySize(sInt attr)
{
sInt size;
size = 0;
if(sDiskItemTypes[attr][0]==sDIT_BINARY && Support(attr))
GetAttr(attr,0,size);
return size;
}
sInt sDiskItem::GetInt(sInt attr)
{
sInt value;
sInt size = 4;
value = 0;
if(sDiskItemTypes[attr][0]==sDIT_INT && Support(attr))
GetAttr(attr,&value,size);
return value;
}
sF32 sDiskItem::GetFloat(sInt attr)
{
sF32 value;
sInt size = 4;
value = 0;
if(sDiskItemTypes[attr][0]==sDIT_FLOAT && Support(attr))
GetAttr(attr,&value,size);
return value;
}
static struct ClosestPoints
EPARecurse(const struct SupportContext *ctx, const int count, const struct MinkowskiPoint *hull, const int iteration)
{
int mini = 0;
cpFloat minDist = INFINITY;
// TODO: precalculate this when building the hull and save a step.
for(int j=0, i=count-1; j<count; i=j, j++){
cpFloat d = ClosestDist(hull[i].ab, hull[j].ab);
if(d < minDist){
minDist = d;
mini = i;
}
}
struct MinkowskiPoint v0 = hull[mini];
struct MinkowskiPoint v1 = hull[(mini + 1)%count];
cpAssertSoft(!cpveql(v0.ab, v1.ab), "Internal Error: EPA vertexes are the same (%d and %d)", mini, (mini + 1)%count);
struct MinkowskiPoint p = Support(ctx, cpvperp(cpvsub(v1.ab, v0.ab)));
#if DRAW_EPA
cpVect verts[count];
for(int i=0; i<count; i++) verts[i] = hull[i].ab;
ChipmunkDebugDrawPolygon(count, verts, 0.0, RGBAColor(1, 1, 0, 1), RGBAColor(1, 1, 0, 0.25));
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5, p.ab, LAColor(1, 1));
#endif
cpFloat area2x = cpvcross(cpvsub(v1.ab, v0.ab), cpvadd(cpvsub(p.ab, v0.ab), cpvsub(p.ab, v1.ab)));
if(area2x > 0.0f && iteration < MAX_EPA_ITERATIONS){
int count2 = 1;
struct MinkowskiPoint *hull2 = (struct MinkowskiPoint *)alloca((count + 1)*sizeof(struct MinkowskiPoint));
hull2[0] = p;
for(int i=0; i<count; i++){
int index = (mini + 1 + i)%count;
cpVect h0 = hull2[count2 - 1].ab;
cpVect h1 = hull[index].ab;
cpVect h2 = (i + 1 < count ? hull[(index + 1)%count] : p).ab;
// TODO: Should this be changed to an area2x check?
if(cpvcross(cpvsub(h2, h0), cpvsub(h1, h0)) > 0.0f){
hull2[count2] = hull[index];
count2++;
}
}
return EPARecurse(ctx, count2, hull2, iteration + 1);
} else {
cpAssertWarn(iteration < WARN_EPA_ITERATIONS, "High EPA iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
}
bool SteerLib::GJK_EPA::GJK(const std::vector<Util::Vector>& _shapeA, const std::vector<Util::Vector>& _shapeB, std::vector<Util::Vector>& _simplex)
{
Util::Vector d(1, 0, -1);
_simplex.push_back(Support(_shapeA, _shapeB, d));
d = d * -1;
while (true) {
_simplex.push_back(Support(_shapeA, _shapeB, d));
if (_simplex.back() * d <= 0) {
return false;
}
else {
if (Origins(_simplex, d))
return true;
}
}
return false;
}
Vec3f GJKDistance(vector<Vec3f>&A, vector<Vec3f>&B, Simplex& P){
P.clearSimplex();
Vec3f v= Support(A, B, A[0] - B[0],P);
P.Add(v);
v = ClosestIn(P);
float lastDist = FLT_MAX;
Simplex lastP;
lastP.SetToSimplex(P);
Vec3f lastV = v;
float epsilon = 0.1;
while(true){
float dist = v.norm();
Vec3f w = Support(A, B, -v, P);
Vector3f vE(v[0], v[1], v[2]);
Vector3f wE(w[0], w[1], w[2]);
float f = dist - (dist - w.norm());
if(f<= tolerance*dist || dist<tolerance){
return v;
}if(lastDist-dist<= epsilon*lastDist){
P.SetToSimplex(lastP);
return lastV;
}else{
lastP.SetToSimplex(P);
lastV = v;
}
if(P.alreadyIn(w))
return v;
if(vE.dot(wE) > 0)return v;
P.Add(w);
v = ClosestIn(P);
P.DeleteNonClosestIn();
if(P.GetSize() > 3) return v; //Should never reach here.
}
return v;
}
// Recursive implementatino of the GJK loop.
static inline struct ClosestPoints
GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration)
{
if(iteration > MAX_GJK_ITERATIONS) {
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
cpVect delta = cpvsub(v1.ab, v0.ab);
// TODO: should this be an area2x check?
if(cpvcross(delta, cpvadd(v0.ab, v1.ab)) > 0.0f) {
// Origin is behind axis. Flip and try again.
return GJKRecurse(ctx, v1, v0, iteration);
} else {
cpFloat t = ClosestT(v0.ab, v1.ab);
cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(delta) : cpvneg(LerpT(v0.ab, v1.ab, t)));
struct MinkowskiPoint p = Support(ctx, n);
#if DRAW_GJK
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1));
cpVect c = cpvlerp(v0.ab, v1.ab, 0.5);
ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1));
#endif
if(
cpvcross(cpvsub(v1.ab, p.ab), cpvadd(v1.ab, p.ab)) > 0.0f &&
cpvcross(cpvsub(v0.ab, p.ab), cpvadd(v0.ab, p.ab)) < 0.0f
) {
// The triangle v0, p, v1 contains the origin. Use EPA to find the MSA.
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration);
return EPA(ctx, v0, p, v1);
} else {
if(cpvdot(p.ab, n) <= cpfmax(cpvdot(v0.ab, n), cpvdot(v1.ab, n))) {
// The edge v0, v1 that we already have is the closest to (0, 0) since p was not closer.
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
} else {
// p was closer to the origin than our existing edge.
// Need to figure out which existing point to drop.
if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)) {
return GJKRecurse(ctx, v0, p, iteration + 1);
} else {
return GJKRecurse(ctx, p, v1, iteration + 1);
}
}
}
}
}
bool SteerLib::GJK_EPA::GJK(const std::vector<Util::Vector>& _shapeA, const std::vector<Util::Vector>& _shapeB, std::vector<Util::Vector>& simplex)
{
Util::Vector d(1, 0, 0);
simplex.push_back(Support(_shapeA, _shapeB, d));
// Negates the vector
d = -d;
while (true)
{
simplex.push_back(Support(_shapeA, _shapeB, d));
//simplex.add
if ((simplex[simplex.size()-1]*d <= 0.0f)) // vector did not pass the origin, it is impossible for the origin to be in the mink. diff.
{
return false;
} else {
if (hasOrigin(simplex,d))
{
return true;
}
}
}
}
static inline struct ClosestPoints
GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration)
{
if(iteration > MAX_GJK_ITERATIONS){
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
cpVect delta = cpvsub(v1.ab, v0.ab);
if(cpvcross(delta, cpvadd(v0.ab, v1.ab)) > 0.0f){
// Origin is behind axis. Flip and try again.
return GJKRecurse(ctx, v1, v0, iteration + 1);
} else {
cpFloat t = ClosestT(v0.ab, v1.ab);
cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(delta) : cpvneg(LerpT(v0.ab, v1.ab, t)));
struct MinkowskiPoint p = Support(ctx, n);
#if DRAW_GJK
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1));
cpVect c = cpvlerp(v0.ab, v1.ab, 0.5);
ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1));
#endif
if(
cpvcross(cpvsub(v1.ab, p.ab), cpvadd(v1.ab, p.ab)) > 0.0f &&
cpvcross(cpvsub(v0.ab, p.ab), cpvadd(v0.ab, p.ab)) < 0.0f
){
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration);
// The triangle v0, p, v1 contains the origin. Use EPA to find the MSA.
return EPA(ctx, v0, p, v1);
} else {
// The new point must be farther along the normal than the existing points.
if(cpvdot(p.ab, n) <= cpfmax(cpvdot(v0.ab, n), cpvdot(v1.ab, n))){
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
} else {
if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)){
return GJKRecurse(ctx, v0, p, iteration + 1);
} else {
return GJKRecurse(ctx, p, v1, iteration + 1);
}
}
}
}
}
WebResponse* ServiceImpl::Execute(WebRequest* pRequest)
{
const char* engine = pRequest->GetEngine();
if(!Support(engine))
{
char msg[AUGE_NAME_MAX];
g_sprintf(msg, "Service [%s] doesn't support Action [%s].", m_name.c_str(), engine);
GLogger* pLogger = augeGetLoggerInstance();
pLogger->Error(msg, __FILE__, __LINE__);
WebExceptionResponse* pExpResponse = augeCreateWebExceptionResponse();
pExpResponse->SetMessage(msg);
return pExpResponse;
}
WebEngine* pWebEngine = NULL;
WebEngineManager* pWebEngingManager = NULL;
pWebEngingManager = augeGetWebEngineManagerInstance();
pWebEngine = pWebEngingManager->GetEngine(engine);
if(pWebEngine==NULL)
{
GError* pError = augeGetErrorInstance();
const char* msg = pError->GetLastError();
WebExceptionResponse* pExpResponse = augeCreateWebExceptionResponse();
pExpResponse->SetMessage(msg);
return pExpResponse;
}
WebContext* pWebContext = augeGetWebContextInstance();
pWebContext->SetService(GetName());
pWebContext->SetURI(GetURI());
//return pWebEngine->Execute(pRequest, pWebContext, GetMap());
return pWebEngine->Execute(pRequest, pWebContext, NULL);
}
void PolyhedronColliderGeometry::UpdateAABB(const RigidBody &body, Proxy &proxy)
{
// POSSIBLE OPTIMIZATION: use cached local AABB to produce a fat AABB
const Math::Vector3 k_xNeg = body.GlobalToLocalVec(Math::Vector3(-1.0f, 0.0, 0.0f));
const Math::Vector3 k_yNeg = body.GlobalToLocalVec(Math::Vector3(0.0f, -1.0f, 0.0f));
const Math::Vector3 k_zNeg = body.GlobalToLocalVec(Math::Vector3(0.0f, 0.0f, -1.0f));
const Math::Vector3 k_xPos = body.GlobalToLocalVec(Math::Vector3(1.0f, 0.0f, 0.0f));
const Math::Vector3 k_yPos = body.GlobalToLocalVec(Math::Vector3(0.0f, 1.0f, 0.0f));
const Math::Vector3 k_zPos = body.GlobalToLocalVec(Math::Vector3(0.0f, 0.0f, 1.0f));
CMeshRenderer* mesh = static_cast<CMeshRenderer*>(mParent->Parent().mParent->cphy->gameObject->GetComponentByName("CMeshRenderer"));
//CTransform* ct = mParent->Parent().mParent->cphy->gameObject->transform;
//
//AABB aabb = mesh->GetMesh()->GetAABB();
//proxy.maxPoint = aabb.maxPoint * ct->scale + ct->position;
//proxy.minPoint = aabb.minPoint * ct->scale + ct->position;
//if (mesh)
//mAdjacency.mCentroid = mesh->GetMesh()->GetOrigin();
Vector3 s = body.mScale;
Matrix3 r = body.mOrientation;
Vector3 t = body.mPosition;
Vector3 xNeg = body.LocalToGlobal(Support(k_xNeg));
Vector3 yNeg = body.LocalToGlobal(Support(k_yNeg));
Vector3 zNeg = body.LocalToGlobal(Support(k_zNeg));
proxy.minPoint.Set(xNeg.x,
yNeg.y,
zNeg.z);
proxy.maxPoint.Set(body.LocalToGlobal(Support(k_xPos)).x,
body.LocalToGlobal(Support(k_yPos)).y,
body.LocalToGlobal(Support(k_zPos)).z);
}
cpVect *hullVerts = alloca(mdiffCount*sizeof(cpVect));
int hullCount = cpConvexHull(mdiffCount, mdiffVerts, hullVerts, NULL, 0.0);
ChipmunkDebugDrawPolygon(hullCount, hullVerts, 0.0, RGBAColor(1, 0, 0, 1), RGBAColor(1, 0, 0, 0.25));
#endif
struct MinkowskiPoint v0, v1;
if(*id) {
// Use the minkowski points from the last frame as a starting point using the cached indexes.
v0 = MinkowskiPointNew(ShapePoint(ctx->shape1, (*id>>24)&0xFF), ShapePoint(ctx->shape2, (*id>>16)&0xFF));
v1 = MinkowskiPointNew(ShapePoint(ctx->shape1, (*id>> 8)&0xFF), ShapePoint(ctx->shape2, (*id )&0xFF));
} else {
// No cached indexes, use the shapes' bounding box centers as a guess for a starting axis.
cpVect axis = cpvperp(cpvsub(cpBBCenter(ctx->shape1->bb), cpBBCenter(ctx->shape2->bb)));
v0 = Support(ctx, axis);
v1 = Support(ctx, cpvneg(axis));
}
struct ClosestPoints points = GJKRecurse(ctx, v0, v1, 1);
*id = points.id;
return points;
}
//MARK: Contact Clipping
// Given two support edges, find contact point pairs on their surfaces.
static inline void
ContactPoints(const struct Edge e1, const struct Edge e2, const struct ClosestPoints points, struct cpCollisionInfo *info)
{
cpFloat mindist = e1.r + e2.r;
Support operator + ( const Support & s1, const Support & s2 ) { return Support( s1.presence + s2.presence, s1.supp + s2.supp ) ;}
void sDiskItem::SetString(sInt attr,sChar *buffer)
{
if(sDiskItemTypes[attr][0]==sDIT_STRING && Support(attr))
SetAttr(attr,buffer,sGetStringLen(buffer)+1);
}
void DebuggerMenuHandler::OnUpdateUI(wxUpdateUIEvent& event)
{
cbProject* prj = Manager::Get()->GetProjectManager()->GetActiveProject();
bool en = false, stopped = false, isRunning = false, isAttached = false;
if (m_activeDebugger)
{
isAttached = m_activeDebugger->IsAttachedToProcess();
en = (prj && !prj->GetCurrentlyCompilingTarget()) || isAttached;
stopped = m_activeDebugger->IsStopped() && !m_activeDebugger->IsBusy();
isRunning = m_activeDebugger->IsRunning();
}
cbEditor* ed = Manager::Get()->GetEditorManager()->GetBuiltinActiveEditor();
wxMenuBar* mbar = Manager::Get()->GetAppFrame()->GetMenuBar();
cbPlugin *runningPlugin = Manager::Get()->GetProjectManager()->GetIsRunning();
bool otherPlugin = false;
if (runningPlugin != NULL && runningPlugin != m_activeDebugger)
{
en = false;
otherPlugin = true;
}
if (mbar && Manager::Get()->GetDebuggerManager()->HasMenu())
{
bool hasBreaks = Support(m_activeDebugger, cbDebuggerFeature::Breakpoints);
mbar->Enable(idMenuDebug, (!isRunning || stopped) && en);
mbar->Enable(idMenuNext, isRunning && en && stopped);
mbar->Enable(idMenuNextInstr, isRunning && en && stopped);
mbar->Enable(idMenuStepIntoInstr, isRunning && en && stopped);
mbar->Enable(idMenuStep, en && stopped);
mbar->Enable(idMenuStepOut, isRunning && en && stopped);
mbar->Enable(idMenuRunToCursor,
en && ed && stopped && Support(m_activeDebugger, cbDebuggerFeature::RunToCursor));
mbar->Enable(idMenuSetNextStatement,
en && ed && stopped && isRunning && Support(m_activeDebugger, cbDebuggerFeature::SetNextStatement));
mbar->Enable(idMenuToggleBreakpoint, ed && m_activeDebugger && hasBreaks);
mbar->Enable(idMenuRemoveAllBreakpoints, m_activeDebugger && hasBreaks);
mbar->Enable(idMenuSendCommand, isRunning && stopped);
mbar->Enable(idMenuAddSymbolFile, isRunning && stopped);
mbar->Enable(idMenuStop, isRunning && en);
mbar->Enable(idMenuBreak, isRunning && !stopped && en);
mbar->Enable(idMenuAttachToProcess, !isRunning && !otherPlugin && m_activeDebugger);
mbar->Enable(idMenuDetach, isRunning && stopped && isAttached);
wxMenu *activeMenu = GetMenuById(idMenuDebugActive);
if (activeMenu)
{
for (size_t ii = 0; ii < activeMenu->GetMenuItemCount(); ++ii)
activeMenu->Enable(activeMenu->FindItemByPosition(ii)->GetId(), !isRunning);
}
mbar->Enable(idMenuTools, m_activeDebugger && m_activeDebugger->ToolMenuEnabled());
}
// allow other UpdateUI handlers to process this event
// *very* important! don't forget it...
event.Skip();
}
void sDiskItem::GetString(sInt attr,sChar *buffer,sInt max)
{
buffer[0] = 0;
if(sDiskItemTypes[attr][0]==sDIT_STRING && Support(attr))
GetAttr(attr,buffer,max);
}
void sDiskItem::SetFloat(sInt attr,sF32 value)
{
if(sDiskItemTypes[attr][0]==sDIT_FLOAT && Support(attr))
SetAttr(attr,&value,4);
}
bool IsColliding(const Collider &collider_a, const Collider &collider_b, Contacts &contacts)
{
// quick check with the individual collider AABBs for a quick out
if (!collider_a.aabb.Overlap(collider_b.aabb))
return false;
// our simplex for this collision test
Simplex simplex;
// Set initial search direction to the difference of centers
Vector2 d = Vector2(1, -1);//Vector2(collider_b.root_trans.PositionWC() - collider_a.root_trans.PositionWC());
// get the first minkowski difference point
simplex.Add(Support(collider_a, collider_b, d));
// negate the support point, giving us a vector in the direction of the origin
d = -simplex.A().vert;
int count = 0;
// start looping
while (count < 100)
{
// add a new point to the simplex because we haven't terminated yet
simplex.Add(Support(collider_a, collider_b, d));
// see if the simplex is on the correct side of the origin
if (Vector2::OppositeDirection(simplex.A().vert, d))
{
// if the point added last was not past the origin in the direction of d
// then the Minkowski Sum cannot possibly contain the origin since
// the last point added is on the edge of the Minkowski Difference
return false;
}
else
{
// oterwise we need to determine if the origin is in the current simplex
// this function will set the next search direction for us if it fails.
if (simplex.ContainsOrigin(d))
{
// if it does then we know there is a collision
// handle the collision with the EPA algorithm
EPAHandle(collider_a, collider_b, simplex, contacts);
// find the incident edge.
if (contacts.size())
{
auto &it = contacts.back();
it.info.e.edge_a = collider_a.FindIndex(it.normal);
it.info.e.edge_b = collider_b.FindIndex(-it.normal);
}
return true;
}
}
++count;
}
return false;
}
void sDiskItem::SetInt(sInt attr,sInt value)
{
if(sDiskItemTypes[attr][0]==sDIT_INT && Support(attr))
SetAttr(attr,&value,4);
}
void EPAHandle(const Collider &collider_a, const Collider &collider_b,
const Simplex &simplex, Contacts &contacts)
{
std::vector<Simplex::Vert> epa_simplex
= { simplex.vertices[0], simplex.vertices[1], simplex.vertices[2] };
// 100 is for security purposes, preventing an infinite loop
for (int i = 0; i < 100; ++i)
{
// find the edge closest to the origin in the simplex
SimplexEdge edge = EPAFindClosestEdge(epa_simplex);
// find the furthest minkowski difference point in the direction of the normal
Vector2 support, support_A, support_B;
Support(collider_a, collider_b, edge.normal,
support, support_A, support_B);
// find the distance between the point and the edge
float dist = Vector2::Dot(support, edge.normal);
// if we've hit the border of the minkowski difference
if (dist - edge.distance < 0.01f)
{
contacts.push_back(Contact());
auto &contact = contacts.back();
contact.normal = edge.normal;
contact.pen_depth = dist;
// For contact points
// Find the repeating point, if there is no repeating point,
// project onto the edge, and use the homogenous coords to find the contact point.
Simplex::Vert &vert_0 = epa_simplex[edge.index_0],
&vert_1 = epa_simplex[edge.index_1];
// if there are no repitions, we project the origin onto the edge
// to find the contact point
float t;
// get the interval from the x coordinates
if (vert_0.vert.x > 0 && vert_1.vert.x < 0 ||
vert_0.vert.x < 0 && vert_1.vert.x > 0)
{
t = (-vert_0.vert.x) / (vert_1.vert.x - vert_0.vert.x);
}
// get the interval from the y coordinates
else
{
t = (-vert_0.vert.y) / (vert_1.vert.y - vert_0.vert.y);
}
contact.point = vert_0.parent_p0 + t * (vert_1.parent_p0 - vert_0.parent_p0);
return;
}
else
{
// add the point inbetween the points where it was found
epa_simplex.insert(epa_simplex.begin() + edge.index_1,
{ support_A, support_B, support });
}
}
}
