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