void physics_system::contact_listener::EndContact(b2Contact* contact) {
auto& sys = get_sys();
auto& cosmos = cosm;
for (int i = 0; i < 2; ++i) {
auto fix_a = contact->GetFixtureA();
auto fix_b = contact->GetFixtureB();
if (i == 1)
std::swap(fix_a, fix_b);
auto body_a = fix_a->GetBody();
auto body_b = fix_b->GetBody();
messages::collision_message msg;
msg.type = messages::collision_message::event_type::END_CONTACT;
auto subject = cosmos[fix_a->GetUserData()];
auto collider = cosmos[fix_b->GetUserData()];
msg.subject = subject;
msg.collider = collider;
auto& subject_fixtures = subject.get<components::fixtures>();
auto& collider_fixtures = collider.get<components::fixtures>();
auto& collider_physics = collider_fixtures.get_owner_body().get<components::special_physics>();
if (subject_fixtures.is_friction_ground() && during_step) {
#if FRICTION_FIELDS_COLLIDE
if (!collider_fixtures.is_friction_ground)
#endif
{
for (auto it = collider_physics.owner_friction_grounds.begin(); it != collider_physics.owner_friction_grounds.end(); ++it)
if ((*it).target == subject_fixtures.get_owner_body())
{
auto& fixtures_connected = (*it).fixtures_connected;
ensure(fixtures_connected > 0);
--fixtures_connected;
if (fixtures_connected == 0) {
LOG("Unreg: %x", subject_fixtures.get_owner_body().get_id());
collider_physics.owner_friction_grounds.erase(it);
sys.rechoose_owner_friction_body(collider_fixtures.get_owner_body());
}
else {
LOG("Decr: %x", subject_fixtures.get_owner_body().get_id());
}
break;
}
}
}
msg.subject_impact_velocity = -body_a->GetLinearVelocity();
msg.collider_impact_velocity = -body_b->GetLinearVelocity();
sys.accumulated_messages.push_back(msg);
}
}
int main(int ac, char **av, char **env)
{
t_system sys;
(void)av;
(void)ac;
if (signal(SIGINT, &handle_sig) == SIG_ERR)
return (EXIT_FAILURE);
if (init_sys(&sys, &sys.history, env) == -1)
return (EXIT_FAILURE);
get_sys(&sys);
process_conf_file(&sys);
aff_prompt(&sys);
get_cmd_line(&sys);
unset_termcaps(&sys);
free_all(&sys);
if (sys.exit.exit == true)
return (sys.exit.value);
return (EXIT_SUCCESS);
}
physics_system::contact_listener::~contact_listener() {
get_sys().b2world->SetContactListener(nullptr);
}
physics_system::contact_listener::contact_listener(cosmos& cosm) : cosm(cosm) {
get_sys().b2world->SetContactListener(this);
}
void physics_system::contact_listener::PreSolve(b2Contact* contact, const b2Manifold* oldManifold) {
auto& sys = get_sys();
auto& cosmos = cosm;
messages::collision_message msgs[2];
for (int i = 0; i < 2; ++i) {
auto fix_a = contact->GetFixtureA();
auto fix_b = contact->GetFixtureB();
if (i == 1)
std::swap(fix_a, fix_b);
b2WorldManifold manifold;
contact->GetWorldManifold(&manifold);
auto body_a = fix_a->GetBody();
auto body_b = fix_b->GetBody();
auto& msg = msgs[i];
msg.type = messages::collision_message::event_type::PRE_SOLVE;
auto subject = cosmos[fix_a->GetUserData()];
auto collider = cosmos[fix_b->GetUserData()];
msg.subject = subject;
msg.collider = collider;
auto& subject_fixtures = subject.get<components::fixtures>();
auto& collider_fixtures = collider.get<components::fixtures>();
if (subject_fixtures.is_friction_ground()) {
// friction fields do not collide with their children
if (are_connected_by_friction(collider, subject)) {
contact->SetEnabled(false);
return;
}
auto& collider_physics = collider_fixtures.get_owner_body().get<components::special_physics>();
for (auto it = collider_physics.owner_friction_grounds.begin(); it != collider_physics.owner_friction_grounds.end(); ++it)
if ((*it).target == subject_fixtures.get_owner_body())
{
contact->SetEnabled(false);
return;
}
}
const const_entity_handle subject_owner_body = subject.get_owner_body();
const const_entity_handle collider_owner_body = collider.get_owner_body();
auto* driver = subject_owner_body.find<components::driver>();
bool colliding_with_owning_car = driver && driver->owned_vehicle == collider_owner_body;
if (colliding_with_owning_car) {
contact->SetEnabled(false);
return;
}
if (subject_fixtures.standard_collision_resolution_disabled() || collider_fixtures.standard_collision_resolution_disabled()) {
contact->SetEnabled(false);
}
msg.subject_collider_and_convex_indices = sys.map_fixture_pointer_to_indices(fix_a, subject);
msg.point = manifold.points[0];
msg.point *= METERS_TO_PIXELSf;
msg.subject_impact_velocity = body_a->GetLinearVelocityFromWorldPoint(manifold.points[0]);
msg.collider_impact_velocity = body_b->GetLinearVelocityFromWorldPoint(manifold.points[0]);
}
sys.accumulated_messages.push_back(msgs[0]);
sys.accumulated_messages.push_back(msgs[1]);
}
void physics_system::contact_listener::BeginContact(b2Contact* contact) {
auto& sys = get_sys();
auto& cosmos = cosm;
auto delta = cosm.get_fixed_delta();
for (int i = 0; i < 2; ++i) {
auto fix_a = contact->GetFixtureA();
auto fix_b = contact->GetFixtureB();
int numPoints = contact->GetManifold()->pointCount;
b2WorldManifold worldManifold;
contact->GetWorldManifold(&worldManifold);
if (i == 1) {
std::swap(fix_a, fix_b);
if (numPoints > 1) {
std::swap(worldManifold.points[0], worldManifold.points[1]);
std::swap(worldManifold.separations[0], worldManifold.separations[1]);
}
worldManifold.normal *= -1;
}
auto body_a = fix_a->GetBody();
auto body_b = fix_b->GetBody();
messages::collision_message msg;
if (fix_a->IsSensor() || fix_b->IsSensor())
msg.one_is_sensor = true;
msg.type = messages::collision_message::event_type::BEGIN_CONTACT;
auto subject = cosmos[fix_a->GetUserData()];
auto collider = cosmos[fix_b->GetUserData()];
msg.subject = subject;
msg.collider = collider;
auto& subject_fixtures = subject.get<components::fixtures>();
auto& collider_fixtures = collider.get<components::fixtures>();
if (subject_fixtures.is_friction_ground()) {
#if FRICTION_FIELDS_COLLIDE
if (!collider_fixtures.is_friction_ground)
#endif
{
auto& collider_physics = collider_fixtures.get_owner_body().get<components::special_physics>();
bool found_suitable = false;
// always accept my own children
if (are_connected_by_friction(collider, subject)) {
found_suitable = true;
}
else if (collider_physics.since_dropped.lasts(cosm.get_timestamp(), delta)) {
collider_physics.since_dropped.unset();
found_suitable = true;
}
else {
for (int i = 0; i < 1; i++) {
const b2Vec2 pointVelOther = body_b->GetLinearVelocityFromWorldPoint(worldManifold.points[i]);
const auto velOtherPixels = vec2(pointVelOther) * METERS_TO_PIXELSf;
if (velOtherPixels.length() > 1) {
const auto angle = vec2(worldManifold.normal).degrees_between(velOtherPixels);
if (angle > 90) {
found_suitable = true;
}
}
if (augs::renderer::get_current().debug_draw_friction_field_collisions_of_entering) {
augs::renderer::get_current().blink_lines.draw_yellow(METERS_TO_PIXELSf*worldManifold.points[i], METERS_TO_PIXELSf* worldManifold.points[i] + vec2(worldManifold.normal).set_length(150));
augs::renderer::get_current().blink_lines.draw_red(METERS_TO_PIXELSf*worldManifold.points[i], METERS_TO_PIXELSf* worldManifold.points[i] + velOtherPixels);
}
}
}
if (found_suitable) {
auto new_owner = subject_fixtures.get_owner_body().get_id();
auto& grounds = collider_physics.owner_friction_grounds;
components::special_physics::friction_connection connection(new_owner);
connection.fixtures_connected = 1;
if (found_in(grounds, new_owner)) {
auto found = find_in(grounds, new_owner);
LOG("Incr: %x", new_owner);
connection.fixtures_connected = (*found).fixtures_connected + 1;
grounds.erase(found);
}
else {
LOG("Reg: %x", new_owner);
}
grounds.push_back(connection);
sys.rechoose_owner_friction_body(collider_fixtures.get_owner_body());
}
}
}
msg.point = worldManifold.points[0];
msg.point *= METERS_TO_PIXELSf;
msg.subject_impact_velocity = body_a->GetLinearVelocityFromWorldPoint(worldManifold.points[0]);
msg.collider_impact_velocity = body_b->GetLinearVelocityFromWorldPoint(worldManifold.points[0]);
sys.accumulated_messages.push_back(msg);
}
}
/*
* Build the contractors
*/
Ctc& StrategyParam::get_ctc() {
Ctc* ctc;
System& ext_sys = get_ext_sys(); // <=> original system in case of a solver
// the first contractor called
Ctc& hc4=rec(new CtcHC4(ext_sys,ratio_propag,hc4_incremental));
// Build contractor #2:
// --------------------------
// An interval Newton iteration
// for solving f()=0 where f is
// a vector-valued function representing
// the system.
Ctc* ctcnewton= NULL;
if (filtering == "acidhc4n" || filtering=="hc4n" || filtering=="3bcidhc4n")
ctcnewton= &rec(new CtcNewton(get_sys().f, NEWTON_CEIL, prec, GAUSS_SEIDEL_RATIO));
if (filtering=="hc4" || filtering=="hc4n") {
ctc = (!ctcnewton)?
&hc4 :
&rec(new CtcCompo(hc4,ctcnewton));
} else {
// hc4 inside acid and 3bcid : incremental propagation beginning with the shaved variable
Ctc& hc44cid=rec(new CtcHC4(ext_sys.ctrs,0.1,true));
if (filtering=="acidhc4" || filtering=="acidhc4n") {
// The ACID contractor (component of the contractor when filtering == "acidhc4")
Ctc& acidhc4=rec(new CtcAcid(ext_sys,hc44cid,optim));
// hc4 followed by acidhc4 : the actual contractor used when filtering == "acidhc4"
ctc=(!ctcnewton)?
&rec(new CtcCompo(hc4, acidhc4)) :
&rec(new CtcCompo(hc4, acidhc4, *ctcnewton));
}
else if (filtering =="3bcidhc4" || filtering =="3bcidhc4n") {
// The 3BCID contractor on all variables (component of the contractor when filtering == "3bcidhc4")
Ctc& c3bcidhc4=rec(new Ctc3BCid(hc44cid));
// hc4 followed by 3bcidhc4 : the actual contractor used when filtering == "3bcidhc4"
ctc=(!ctcnewton)?
&rec(new CtcCompo(hc4, c3bcidhc4)) :
&rec(new CtcCompo(hc4, c3bcidhc4, *ctcnewton));
}
else {
ibex_error("StrategyParam: unknown contractor mode");
}
}
// The CtcXNewton contractor
// corner selection for linearizations : two corners are selected, a random one and its opposite
vector<LinearRelaxXTaylor::corner_point> cpoints;
cpoints.push_back(LinearRelaxXTaylor::RANDOM);
cpoints.push_back(LinearRelaxXTaylor::RANDOM_INV);
LinearRelax* lr;
if (lin_relax=="art")
lr = &rec(new LinearRelaxCombo(ext_sys,LinearRelaxCombo::ART));
else if (lin_relax=="compo")
lr = &rec(new LinearRelaxCombo(ext_sys,LinearRelaxCombo::COMPO));
else if (lin_relax=="xn")
lr = &rec(new LinearRelaxXTaylor(ext_sys,cpoints));
else
ibex_error("StrategyParam: unknown liner relaxation mode");
// fixpoint linear relaxation , hc4 with default fix point ratio 0.2
if (lin_relax=="compo" || lin_relax=="art"|| lin_relax=="xn") {
//cxn = new CtcLinearRelaxation (*lr, hc44xn);
Ctc& cxn_poly = rec(new CtcPolytopeHull(*lr, CtcPolytopeHull::ALL_BOX));
// hc4 inside xnewton loop
Ctc& hc44xn = rec(new CtcHC4(ext_sys.ctrs,ratio_propag,false));
Ctc& cxn_compo = rec(new CtcCompo(cxn_poly, hc44xn));
Ctc& cxn = rec(new CtcFixPoint (cxn_compo, fixpoint_ratio));
// the actual contractor ctc + linear relaxation
return rec(new CtcCompo (*ctc, cxn));
}
else
return *ctc;
}
System& OptimizerParam::get_ext_sys() {
if ((*memory())->sys.size()==2) {
return (ExtendedSystem&) *((*memory())->sys.back()); // already built and recorded
}
else return rec(new ExtendedSystem(get_sys(),eq_eps));
}
System& StrategyParam::get_ext_sys() {
return get_sys();
}
