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 }); } } }