query_result_chassis_vec_t* PhysicsRegion::queryLine(const point_t src, const point_t dst, flag_plane p) { struct : public rects::QueryCallback { PhysicsRegion* parent; std::set<std::pair<float,chassis_id>> query_result; point_t src; bool onMatch(rects::Rect* r, point_t at) { chassis_id ch = parent->getChassisFromRect(r); float dist = (at-src).getMagnitude(); query_result.insert({dist,ch}); return true; } } qc; qc.src=src; qc.parent = this; for (byte i = 0; i < getPlaneCount(); i++) { flag_plane p_it = 1 << i; if (p_it & p) planes[i].queryLine(src, dst, &qc); } query_result_chassis_vec.clear(); for (auto iter : qc.query_result) query_result_chassis_vec.push_back(iter.second); return &query_result_chassis_vec; }
bool ConvexHull<N, T>::contains(const AABBType& aabb) const { const VectorType min = aabb.getMin(); const VectorType max = aabb.getMax(); for (std::size_t i = 0; i < getPlaneCount(); ++i) { const VectorType& normal = planes[i].getNormal(); VectorType pv; VectorType nv; for (std::size_t j = 0; j < N; ++j) { pv[j] = normal[j] > ScalarType(0) ? max[j] : min[j]; nv[j] = normal[j] < ScalarType(0) ? max[j] : min[j]; } if (planes[i].distance(pv) < ScalarType(0) || planes[i].distance(nv) < ScalarType(0)) { return false; } } return true; }
chassis_id PhysicsRegion::addChassis(point_t pin, PhysicsCallback* callback) { chassis_t chas; chas.pin=pin; chas.callback=callback; chas.limbs.resize(getPlaneCount(),NO_LIMB); chassis_id id = chassis.size(); chassis.push_back(chas); return id; }
void PhysicsRegion::setLimbBounds(chassis_id id, rectangleF bounds, flag_plane p) { assert(id != NO_CHASSIS); for (byte i=0;i<getPlaneCount();i++) { flag_plane p_it = 1<<i; if (p&p_it) setLimbBounds(chassis[id].limbs[i],bounds); } }
void PhysicsRegion::drawLimbs(Renderer* r, flag_plane p) { for (int i = 0; i < getPlaneCount(); i++) { color c = boundsColours[i % sizeof(boundsColours)]; for (auto iter : *getLimbList(1 << i)) if (!limbs[iter].tombstone) r->addPainter(Painter::makeRectangle(getLimbBounds(iter), c, false), PHYSICS_LIMB_DRAW_DEPTH - i); } }
bool ConvexHull<N, T>::contains(const SphereType& sphere) const { for (std::size_t i = 0; i < getPlaneCount(); ++i) { if (planes[i].distance(sphere.getPosition()) < sphere.getRadius()) return false; } return false; }
bool ConvexHull<N, T>::contains(const VectorType& point) const { for (std::size_t i = 0; i < getPlaneCount(); ++i) { if (planes[i].distance(point) < ScalarType(0)) return false; } return true; }
void PhysicsRegion::setChassisCoord(chassis_id id, point_t coord) { assert(id != NO_CHASSIS); chassis[id].pin=coord; for (int p=0;p<getPlaneCount();p++) { limb_id lid = getLimb(id,1<<p); if (lid==NO_LIMB) continue; point_t coord_l = coord+limbs[lid].offset; limbs[lid].bounds->setPosition(coord_l.getX(),coord_l.getY()); } }
query_result_limb_t* PhysicsRegion::getLimbs(chassis_id id, flag_plane p) { query_result_limb.clear(); assert(id != NO_CHASSIS); for (byte i = 0; i < getPlaneCount(); i++) { flag_plane p_it = 1 << i; if (p & p_it) if (chassis[id].limbs[i]!=NO_LIMB) query_result_limb.insert(chassis[id].limbs[i]); } return &query_result_limb; }
limb_id PhysicsRegion::getLimb(chassis_id id, flag_plane p) const { assert(id != NO_CHASSIS); assert(!chassis[id].tombstone); for (byte i = 0; i < getPlaneCount(); i++) { flag_plane p_it = 1 << i; limb_id to_return = chassis[id].limbs[i]; if ((p & p_it) && to_return != NO_LIMB) return to_return; } return NO_LIMB; }
query_result_chassis_t* PhysicsRegion::getChassisList(flag_plane p) { query_result_chassis.clear(); for (unsigned int it_ch = 0; it_ch < chassis.size(); it_ch++) { for (unsigned int i = 0; i < getPlaneCount(); i++) { flag_plane p_it = 1 << i; if ((p & p_it) && chassis[it_ch].limbs[i] != NO_LIMB) { query_result_chassis.insert(it_ch); break; //to next chassis } } } return &query_result_chassis; }
bool ConvexHull<N, T>::intersects(const SphereType& sphere) const { const VectorType& position = sphere.getPosition(); const ScalarType radius = -sphere.getRadius(); for (std::size_t i = 0; i < getPlaneCount(); ++i) { if (planes[i].distance(position) < radius) return false; } return true; }
bool PhysicsRegion::testRectangle(rectangleF r, chassis_id id, flag_plane p) { for (byte i = 0; i < getPlaneCount(); i++) { flag_plane p_it = 1 << i; if (!p_it & p) continue; if (chassis[id].limbs[i] == NO_LIMB) continue; rectangleF rect = getLimbBounds(chassis[id].limbs[p]); if (rect.intersects(r)) return true; } return false; }
bool PhysicsRegion::testIntersectOffset(chassis_id first, chassis_id second, vector2F offset, flag_plane p) { for (byte i = 0; i < getPlaneCount(); i++) { flag_plane p_it = 1 << i; if (!p_it & p) continue; if (chassis[first].limbs[i] == NO_LIMB) continue; rectangleF rect = getLimbBounds(chassis[first].limbs[p])+offset; rectangleF rect2 = getLimbBounds(chassis[second].limbs[p]); if (rect.intersects(rect2)) return true; } return false; }
chassis_id PhysicsRegion::moveAttemptRelative(chassis_id id, vector2F offset, flag_plane src, flag_plane collision) { for (byte p = 0;p<getPlaneCount();p++) { flag_plane p_it = 1<<p; if (!(p_it&src)) continue; limb_id lid = getLimb(id,p_it); if (lid==NO_LIMB) continue; rectangleF rect = getLimbBounds(lid)+offset; chassis_id obs = queryFirstRectangle(rect,collision); if (obs!=NO_CHASSIS) return obs; } setChassisCoord(id,getChassisCoord(id)+offset); return NO_CHASSIS; }
chassis_id PhysicsRegion::queryFirstRectangle(rectangleF rect, flag_plane p) { struct : public rects::QueryCallback { const PhysicsRegion* parent; chassis_id ret=NO_CHASSIS; bool onMatch(rects::Rect* r, point_t at) { ret = parent->getChassisFromRect(r); return false; } } qc; qc.parent = this; for (byte i = 0; i < getPlaneCount(); i++) { flag_plane p_it = 1 << i; if (p_it & p) planes[i].queryRectangle(rect.x, rect.y, rect.w, rect.h, &qc); if (qc.ret!=NO_CHASSIS) return qc.ret; } return qc.ret; }
query_result_chassis_t* PhysicsRegion::queryPoint(point_t point, flag_plane p) { struct : public rects::QueryCallback { PhysicsRegion* parent; query_result_chassis_t* query_result; bool onMatch(rects::Rect* r, point_t at) { chassis_id ch = parent->getChassisFromRect(r); query_result->insert(ch); return true; } } qc; qc.parent = this; query_result_chassis.clear(); qc.query_result = &query_result_chassis; for (byte i = 0; i < getPlaneCount(); i++) { flag_plane p_it = 1 << i; if (p_it & p) planes[i].queryPoint(point.getX(), point.getY(), &qc); } return &query_result_chassis; }
limb_id PhysicsRegion::addLimb(chassis_id p, rectangleF bounds, flag_plane plane) { limb_t limb; limb.parent=p; limb_id to_return; for (byte i = 0; i < getPlaneCount(); i++) if ((1 << i) & plane) { //assert no pre-existing limb on this plane attached to chassis. assert(chassis[p].limbs[i]==NO_LIMB); limb_id lid = limbs.size(); limb.plane=i; limb.bounds=new rects::Rect(bounds+getChassisCoord(p)); limb.bounds->setUserDefined(lid); limb.offset={bounds.x,bounds.y}; planes[i].add(limb.bounds); chassis[p].limbs[i]=lid; limbs.push_back(limb); to_return=lid; } return to_return; }
query_result_chassis_t* PhysicsRegion::queryRectangle(rectangleF rect, flag_plane p) { struct : public rects::QueryCallback { PhysicsRegion* parent; query_result_chassis_t* query_result; bool onMatch(rects::Rect* r, point_t at) { chassis_id ch = parent->getChassisFromRect(r); query_result->insert(ch); return true; } } qc; qc.parent = this; query_result_chassis.clear(); qc.query_result = &query_result_chassis; for (byte i = 0; i < getPlaneCount(); i++) { flag_plane p_it = 1 << i; if (p_it & p) planes[i].queryRectangle(rect.x, rect.y, rect.w, rect.h, &qc); } //todo: remove duplicates return &query_result_chassis; }
chassis_id PhysicsRegion::queryFirstIntersectOffset(chassis_id id, vector2F offset, flag_plane p) { struct : public rects::QueryCallback { const PhysicsRegion* parent; chassis_id ret = NO_CHASSIS; bool onMatch(rects::Rect* r, point_t at) { ret = parent->getChassisFromRect(r); return false; } } qc; qc.parent = this; for (byte i = 0; i < getPlaneCount(); i++) { if (chassis[id].limbs[i] == NO_LIMB) continue; flag_plane p_it = 1 << i; rectangleF rect = getLimbBounds(chassis[id].limbs[i])+offset; if (p_it & p) planes[i].queryRectangle(rect.x, rect.y, rect.w, rect.h, &qc); if (qc.ret != NO_CHASSIS) return qc.ret; } return qc.ret; }
query_result_chassis_t* PhysicsRegion::queryIntersectOffset( chassis_id id, vector2F offset, flag_plane p) { struct : public rects::QueryCallback { PhysicsRegion* parent; query_result_chassis_t* query_result; bool onMatch(rects::Rect* r, point_t at) { chassis_id ch = parent->getChassisFromRect(r); query_result->insert(ch); return true; } } qc; qc.parent = this; query_result_chassis.clear(); qc.query_result = &query_result_chassis; for (byte i = 0; i < getPlaneCount(); i++) { if (chassis[id].limbs[i]==NO_LIMB) continue; flag_plane p_it = 1 << i; rectangleF rect = getLimbBounds(chassis[id].limbs[i])+offset; if (p_it & p) planes[i].queryRectangle(rect.x, rect.y, rect.w, rect.h, &qc); } return &query_result_chassis; }