RID DampedSpringJoint2D::_configure_joint() {
Node *node_a = has_node(get_node_a()) ? get_node(get_node_a()) : (Node *)NULL;
Node *node_b = has_node(get_node_b()) ? get_node(get_node_b()) : (Node *)NULL;
if (!node_a || !node_b)
return RID();
PhysicsBody2D *body_a = node_a->cast_to<PhysicsBody2D>();
PhysicsBody2D *body_b = node_b->cast_to<PhysicsBody2D>();
if (!body_a || !body_b)
return RID();
if (get_exclude_nodes_from_collision())
Physics2DServer::get_singleton()->body_add_collision_exception(body_a->get_rid(), body_b->get_rid());
else
Physics2DServer::get_singleton()->body_remove_collision_exception(body_a->get_rid(), body_b->get_rid());
Transform2D gt = get_global_transform();
Vector2 anchor_A = gt.get_origin();
Vector2 anchor_B = gt.xform(Vector2(0, length));
RID dsj = Physics2DServer::get_singleton()->damped_spring_joint_create(anchor_A, anchor_B, body_a->get_rid(), body_b->get_rid());
if (rest_length)
Physics2DServer::get_singleton()->damped_string_joint_set_param(dsj, Physics2DServer::DAMPED_STRING_REST_LENGTH, rest_length);
Physics2DServer::get_singleton()->damped_string_joint_set_param(dsj, Physics2DServer::DAMPED_STRING_STIFFNESS, stiffness);
Physics2DServer::get_singleton()->damped_string_joint_set_param(dsj, Physics2DServer::DAMPED_STRING_DAMPING, damping);
return dsj;
}
bool CollisionSolver2DSW::solve_raycast(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis) {
const RayShape2DSW *ray = static_cast<const RayShape2DSW *>(p_shape_A);
if (p_shape_B->get_type() == Physics2DServer::SHAPE_RAY)
return false;
Vector2 from = p_transform_A.get_origin();
Vector2 to = from + p_transform_A[1] * ray->get_length();
Vector2 support_A = to;
Transform2D invb = p_transform_B.affine_inverse();
from = invb.xform(from);
to = invb.xform(to);
Vector2 p, n;
if (!p_shape_B->intersect_segment(from, to, p, n)) {
if (sep_axis)
*sep_axis = p_transform_A[1].normalized();
return false;
}
Vector2 support_B = p_transform_B.xform(p);
if (p_result_callback) {
if (p_swap_result)
p_result_callback(support_B, support_A, p_userdata);
else
p_result_callback(support_A, support_B, p_userdata);
}
return true;
}
void RayCast2D::_update_raycast_state() {
Ref<World2D> w2d = get_world_2d();
ERR_FAIL_COND(w2d.is_null());
Physics2DDirectSpaceState *dss = Physics2DServer::get_singleton()->space_get_direct_state(w2d->get_space());
ERR_FAIL_COND(!dss);
Transform2D gt = get_global_transform();
Vector2 to = cast_to;
if (to == Vector2())
to = Vector2(0, 0.01);
Physics2DDirectSpaceState::RayResult rr;
if (dss->intersect_ray(gt.get_origin(), gt.xform(to), rr, exclude, collision_layer, type_mask)) {
collided = true;
against = rr.collider_id;
collision_point = rr.position;
collision_normal = rr.normal;
against_shape = rr.shape;
} else {
collided = false;
}
}
RID GrooveJoint2D::_configure_joint() {
Node *node_a = has_node(get_node_a()) ? get_node(get_node_a()) : (Node *)NULL;
Node *node_b = has_node(get_node_b()) ? get_node(get_node_b()) : (Node *)NULL;
if (!node_a || !node_b)
return RID();
PhysicsBody2D *body_a = node_a->cast_to<PhysicsBody2D>();
PhysicsBody2D *body_b = node_b->cast_to<PhysicsBody2D>();
if (!body_a || !body_b)
return RID();
if (get_exclude_nodes_from_collision())
Physics2DServer::get_singleton()->body_add_collision_exception(body_a->get_rid(), body_b->get_rid());
else
Physics2DServer::get_singleton()->body_remove_collision_exception(body_a->get_rid(), body_b->get_rid());
Transform2D gt = get_global_transform();
Vector2 groove_A1 = gt.get_origin();
Vector2 groove_A2 = gt.xform(Vector2(0, length));
Vector2 anchor_B = gt.xform(Vector2(0, initial_offset));
return Physics2DServer::get_singleton()->groove_joint_create(groove_A1, groove_A2, anchor_B, body_a->get_rid(), body_b->get_rid());
}
RID GrooveJoint2D::_configure_joint(PhysicsBody2D *body_a, PhysicsBody2D *body_b) {
Transform2D gt = get_global_transform();
Vector2 groove_A1 = gt.get_origin();
Vector2 groove_A2 = gt.xform(Vector2(0, length));
Vector2 anchor_B = gt.xform(Vector2(0, initial_offset));
return Physics2DServer::get_singleton()->groove_joint_create(groove_A1, groove_A2, anchor_B, body_a->get_rid(), body_b->get_rid());
}
RID DampedSpringJoint2D::_configure_joint(PhysicsBody2D *body_a, PhysicsBody2D *body_b) {
Transform2D gt = get_global_transform();
Vector2 anchor_A = gt.get_origin();
Vector2 anchor_B = gt.xform(Vector2(0, length));
RID dsj = Physics2DServer::get_singleton()->damped_spring_joint_create(anchor_A, anchor_B, body_a->get_rid(), body_b->get_rid());
if (rest_length)
Physics2DServer::get_singleton()->damped_string_joint_set_param(dsj, Physics2DServer::DAMPED_STRING_REST_LENGTH, rest_length);
Physics2DServer::get_singleton()->damped_string_joint_set_param(dsj, Physics2DServer::DAMPED_STRING_STIFFNESS, stiffness);
Physics2DServer::get_singleton()->damped_string_joint_set_param(dsj, Physics2DServer::DAMPED_STRING_DAMPING, damping);
return dsj;
}
bool CollisionSolver2DSW::solve_concave(const Shape2DSW *p_shape_A, const Transform2D &p_transform_A, const Vector2 &p_motion_A, const Shape2DSW *p_shape_B, const Transform2D &p_transform_B, const Vector2 &p_motion_B, CallbackResult p_result_callback, void *p_userdata, bool p_swap_result, Vector2 *sep_axis, real_t p_margin_A, real_t p_margin_B) {
const ConcaveShape2DSW *concave_B = static_cast<const ConcaveShape2DSW *>(p_shape_B);
_ConcaveCollisionInfo2D cinfo;
cinfo.transform_A = &p_transform_A;
cinfo.shape_A = p_shape_A;
cinfo.transform_B = &p_transform_B;
cinfo.motion_A = p_motion_A;
cinfo.result_callback = p_result_callback;
cinfo.userdata = p_userdata;
cinfo.swap_result = p_swap_result;
cinfo.collided = false;
cinfo.collisions = 0;
cinfo.sep_axis = sep_axis;
cinfo.margin_A = p_margin_A;
cinfo.margin_B = p_margin_B;
cinfo.aabb_tests = 0;
Transform2D rel_transform = p_transform_A;
rel_transform.elements[2] -= p_transform_B.get_origin();
//quickly compute a local Rect2
Rect2 local_aabb;
for (int i = 0; i < 2; i++) {
Vector2 axis(p_transform_B.elements[i]);
real_t axis_scale = 1.0 / axis.length();
axis *= axis_scale;
real_t smin, smax;
p_shape_A->project_rangev(axis, rel_transform, smin, smax);
smin *= axis_scale;
smax *= axis_scale;
local_aabb.position[i] = smin;
local_aabb.size[i] = smax - smin;
}
concave_B->cull(local_aabb, concave_callback, &cinfo);
//print_line("Rect2 TESTS: "+itos(cinfo.aabb_tests));
return cinfo.collided;
}
bool Space2DSW::test_body_motion(Body2DSW *p_body, const Transform2D &p_from, const Vector2 &p_motion, real_t p_margin, Physics2DServer::MotionResult *r_result) {
//give me back regular physics engine logic
//this is madness
//and most people using this function will think
//what it does is simpler than using physics
//this took about a week to get right..
//but is it right? who knows at this point..
if (r_result) {
r_result->collider_id = 0;
r_result->collider_shape = 0;
}
Rect2 body_aabb;
for (int i = 0; i < p_body->get_shape_count(); i++) {
if (i == 0)
body_aabb = p_body->get_shape_aabb(i);
else
body_aabb = body_aabb.merge(p_body->get_shape_aabb(i));
}
// Undo the currently transform the physics server is aware of and apply the provided one
body_aabb = p_from.xform(p_body->get_inv_transform().xform(body_aabb));
body_aabb = body_aabb.grow(p_margin);
Transform2D body_transform = p_from;
{
//STEP 1, FREE BODY IF STUCK
const int max_results = 32;
int recover_attempts = 4;
Vector2 sr[max_results * 2];
do {
Physics2DServerSW::CollCbkData cbk;
cbk.max = max_results;
cbk.amount = 0;
cbk.ptr = sr;
Physics2DServerSW::CollCbkData *cbkptr = &cbk;
CollisionSolver2DSW::CallbackResult cbkres = Physics2DServerSW::_shape_col_cbk;
bool collided = false;
int amount = _cull_aabb_for_body(p_body, body_aabb);
for (int j = 0; j < p_body->get_shape_count(); j++) {
if (p_body->is_shape_set_as_disabled(j))
continue;
Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j);
Shape2DSW *body_shape = p_body->get_shape(j);
for (int i = 0; i < amount; i++) {
const CollisionObject2DSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];
if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
cbk.valid_dir = body_shape_xform.get_axis(1).normalized();
cbk.valid_depth = p_margin; //only valid depth is the collision margin
} else {
cbk.valid_dir = Vector2();
cbk.valid_depth = 0;
}
if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), cbkres, cbkptr, NULL, p_margin)) {
collided = cbk.amount > 0;
}
}
}
if (!collided) {
break;
}
Vector2 recover_motion;
for (int i = 0; i < cbk.amount; i++) {
Vector2 a = sr[i * 2 + 0];
Vector2 b = sr[i * 2 + 1];
#if 0
Vector2 rel = b-a;
real_t d = rel.length();
if (d==0)
continue;
Vector2 n = rel/d;
real_t traveled = n.dot(recover_motion);
a+=n*traveled;
real_t d = a.distance_to(b);
if (d<margin)
continue;
#endif
recover_motion += (b - a) * 0.4;
}
if (recover_motion == Vector2()) {
collided = false;
break;
}
body_transform.elements[2] += recover_motion;
body_aabb.position += recover_motion;
recover_attempts--;
} while (recover_attempts);
}
real_t safe = 1.0;
real_t unsafe = 1.0;
int best_shape = -1;
{
// STEP 2 ATTEMPT MOTION
Rect2 motion_aabb = body_aabb;
motion_aabb.position += p_motion;
motion_aabb = motion_aabb.merge(body_aabb);
int amount = _cull_aabb_for_body(p_body, motion_aabb);
for (int j = 0; j < p_body->get_shape_count(); j++) {
if (p_body->is_shape_set_as_disabled(j))
continue;
Transform2D body_shape_xform = body_transform * p_body->get_shape_transform(j);
Shape2DSW *body_shape = p_body->get_shape(j);
bool stuck = false;
real_t best_safe = 1;
real_t best_unsafe = 1;
for (int i = 0; i < amount; i++) {
const CollisionObject2DSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];
Transform2D col_obj_xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
//test initial overlap, does it collide if going all the way?
if (!CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, NULL, 0)) {
continue;
}
//test initial overlap
if (CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, NULL, 0)) {
if (col_obj->is_shape_set_as_one_way_collision(j)) {
continue;
}
stuck = true;
break;
}
//just do kinematic solving
real_t low = 0;
real_t hi = 1;
Vector2 mnormal = p_motion.normalized();
for (int k = 0; k < 8; k++) { //steps should be customizable..
real_t ofs = (low + hi) * 0.5;
Vector2 sep = mnormal; //important optimization for this to work fast enough
bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * ofs, col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), NULL, NULL, &sep, 0);
if (collided) {
hi = ofs;
} else {
low = ofs;
}
}
if (col_obj->is_shape_set_as_one_way_collision(j)) {
Vector2 cd[2];
Physics2DServerSW::CollCbkData cbk;
cbk.max = 1;
cbk.amount = 0;
cbk.ptr = cd;
cbk.valid_dir = body_shape_xform.get_axis(1).normalized();
;
cbk.valid_depth = 10e20;
Vector2 sep = mnormal; //important optimization for this to work fast enough
bool collided = CollisionSolver2DSW::solve(body_shape, body_shape_xform, p_motion * (hi + contact_max_allowed_penetration), col_obj->get_shape(shape_idx), col_obj_xform, Vector2(), Physics2DServerSW::_shape_col_cbk, &cbk, &sep, 0);
if (!collided || cbk.amount == 0) {
continue;
}
}
if (low < best_safe) {
best_safe = low;
best_unsafe = hi;
}
}
if (stuck) {
safe = 0;
unsafe = 0;
best_shape = j; //sadly it's the best
break;
}
if (best_safe == 1.0) {
continue;
}
if (best_safe < safe) {
safe = best_safe;
unsafe = best_unsafe;
best_shape = j;
}
}
}
bool collided = false;
if (safe >= 1) {
//not collided
collided = false;
if (r_result) {
r_result->motion = p_motion;
r_result->remainder = Vector2();
r_result->motion += (body_transform.get_origin() - p_from.get_origin());
}
} else {
//it collided, let's get the rest info in unsafe advance
Transform2D ugt = body_transform;
ugt.elements[2] += p_motion * unsafe;
_RestCallbackData2D rcd;
rcd.best_len = 0;
rcd.best_object = NULL;
rcd.best_shape = 0;
Transform2D body_shape_xform = ugt * p_body->get_shape_transform(best_shape);
Shape2DSW *body_shape = p_body->get_shape(best_shape);
body_aabb.position += p_motion * unsafe;
int amount = _cull_aabb_for_body(p_body, body_aabb);
for (int i = 0; i < amount; i++) {
const CollisionObject2DSW *col_obj = intersection_query_results[i];
int shape_idx = intersection_query_subindex_results[i];
if (col_obj->is_shape_set_as_one_way_collision(shape_idx)) {
rcd.valid_dir = body_shape_xform.get_axis(1).normalized();
rcd.valid_depth = 10e20;
} else {
rcd.valid_dir = Vector2();
rcd.valid_depth = 0;
}
rcd.object = col_obj;
rcd.shape = shape_idx;
bool sc = CollisionSolver2DSW::solve(body_shape, body_shape_xform, Vector2(), col_obj->get_shape(shape_idx), col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), Vector2(), _rest_cbk_result, &rcd, NULL, p_margin);
if (!sc)
continue;
}
if (rcd.best_len != 0) {
if (r_result) {
r_result->collider = rcd.best_object->get_self();
r_result->collider_id = rcd.best_object->get_instance_id();
r_result->collider_shape = rcd.best_shape;
r_result->collision_local_shape = best_shape;
r_result->collision_normal = rcd.best_normal;
r_result->collision_point = rcd.best_contact;
r_result->collider_metadata = rcd.best_object->get_shape_metadata(rcd.best_shape);
const Body2DSW *body = static_cast<const Body2DSW *>(rcd.best_object);
Vector2 rel_vec = r_result->collision_point - body->get_transform().get_origin();
r_result->collider_velocity = Vector2(-body->get_angular_velocity() * rel_vec.y, body->get_angular_velocity() * rel_vec.x) + body->get_linear_velocity();
r_result->motion = safe * p_motion;
r_result->remainder = p_motion - safe * p_motion;
r_result->motion += (body_transform.get_origin() - p_from.get_origin());
}
collided = true;
} else {
if (r_result) {
r_result->motion = p_motion;
r_result->remainder = Vector2();
r_result->motion += (body_transform.get_origin() - p_from.get_origin());
}
collided = false;
}
}
return collided;
}
void Body2DSW::update_inertias() {
//update shapes and motions
switch (mode) {
case Physics2DServer::BODY_MODE_RIGID: {
if (user_inertia) break;
//update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet)
real_t total_area = 0;
for (int i = 0; i < get_shape_count(); i++) {
total_area += get_shape_aabb(i).get_area();
}
real_t _inertia = 0;
for (int i = 0; i < get_shape_count(); i++) {
const Shape2DSW *shape = get_shape(i);
real_t area = get_shape_aabb(i).get_area();
real_t mass = area * this->mass / total_area;
Transform2D mtx = get_shape_transform(i);
Vector2 scale = mtx.get_scale();
_inertia += shape->get_moment_of_inertia(mass, scale) + mass * mtx.get_origin().length_squared();
//Rect2 ab = get_shape_aabb(i);
//_inertia+=mass*ab.size.dot(ab.size)/12.0f;
}
if (_inertia != 0)
_inv_inertia = 1.0 / _inertia;
else
_inv_inertia = 0.0; //wathever
if (mass)
_inv_mass = 1.0 / mass;
else
_inv_mass = 0;
} break;
case Physics2DServer::BODY_MODE_KINEMATIC:
case Physics2DServer::BODY_MODE_STATIC: {
_inv_inertia = 0;
_inv_mass = 0;
} break;
case Physics2DServer::BODY_MODE_CHARACTER: {
_inv_inertia = 0;
_inv_mass = 1.0 / mass;
} break;
}
//_update_inertia_tensor();
//_update_shapes();
}