diff options
Diffstat (limited to 'servers/physics')
35 files changed, 10564 insertions, 0 deletions
diff --git a/servers/physics/SCsub b/servers/physics/SCsub new file mode 100644 index 0000000000..16fe3a59ac --- /dev/null +++ b/servers/physics/SCsub @@ -0,0 +1,7 @@ +Import('env') + +env.add_source_files(env.servers_sources,"*.cpp") + +Export('env') + + diff --git a/servers/physics/area_pair_sw.cpp b/servers/physics/area_pair_sw.cpp new file mode 100644 index 0000000000..4a303d3fdd --- /dev/null +++ b/servers/physics/area_pair_sw.cpp @@ -0,0 +1,94 @@ +/*************************************************************************/ +/* area_pair_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "area_pair_sw.h" +#include "collision_solver_sw.h" + + +bool AreaPairSW::setup(float p_step) { + + bool result = CollisionSolverSW::solve_static(body->get_shape(body_shape),body->get_transform() * body->get_shape_transform(body_shape),area->get_shape(area_shape),area->get_transform() * area->get_shape_transform(area_shape),NULL,this); + + if (result!=colliding) { + + if (result) { + + if (area->get_space_override_mode()!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED) + body->add_area(area); + if (area->has_monitor_callback()) + area->add_body_to_query(body,body_shape,area_shape); + + } else { + + if (area->get_space_override_mode()!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED) + body->remove_area(area); + if (area->has_monitor_callback()) + area->remove_body_from_query(body,body_shape,area_shape); + + } + + colliding=result; + + } + + return false; //never do any post solving +} + +void AreaPairSW::solve(float p_step) { + + +} + + +AreaPairSW::AreaPairSW(BodySW *p_body,int p_body_shape, AreaSW *p_area,int p_area_shape) { + + + body=p_body; + area=p_area; + body_shape=p_body_shape; + area_shape=p_area_shape; + colliding=false; + body->add_constraint(this,0); + area->add_constraint(this); + +} + +AreaPairSW::~AreaPairSW() { + + if (colliding) { + + if (area->get_space_override_mode()!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED) + body->remove_area(area); + if (area->has_monitor_callback()) + area->remove_body_from_query(body,body_shape,area_shape); + + + } + body->remove_constraint(this); + area->remove_constraint(this); +} diff --git a/servers/physics/area_pair_sw.h b/servers/physics/area_pair_sw.h new file mode 100644 index 0000000000..e5e2b5cf5e --- /dev/null +++ b/servers/physics/area_pair_sw.h @@ -0,0 +1,53 @@ +/*************************************************************************/ +/* area_pair_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef AREA_PAIR_SW_H +#define AREA_PAIR_SW_H + +#include "constraint_sw.h" +#include "body_sw.h" +#include "area_sw.h" + +class AreaPairSW : public ConstraintSW { + + BodySW *body; + AreaSW *area; + int body_shape; + int area_shape; + bool colliding; +public: + + bool setup(float p_step); + void solve(float p_step); + + AreaPairSW(BodySW *p_body,int p_body_shape, AreaSW *p_area,int p_area_shape); + ~AreaPairSW(); +}; + +#endif // AREA_PAIR__SW_H + diff --git a/servers/physics/area_sw.cpp b/servers/physics/area_sw.cpp new file mode 100644 index 0000000000..33962d993a --- /dev/null +++ b/servers/physics/area_sw.cpp @@ -0,0 +1,192 @@ +/*************************************************************************/ +/* area_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "area_sw.h" +#include "space_sw.h" +#include "body_sw.h" + +AreaSW::BodyKey::BodyKey(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { rid=p_body->get_self(); instance_id=p_body->get_instance_id(); body_shape=p_body_shape; area_shape=p_area_shape; } + +void AreaSW::_shapes_changed() { + + +} + +void AreaSW::set_transform(const Transform& p_transform) { + + if (!moved_list.in_list() && get_space()) + get_space()->area_add_to_moved_list(&moved_list); + + _set_transform(p_transform); +} + +void AreaSW::set_space(SpaceSW *p_space) { + + if (get_space()) { + if (monitor_query_list.in_list()) + get_space()->area_remove_from_monitor_query_list(&monitor_query_list); + if (moved_list.in_list()) + get_space()->area_remove_from_moved_list(&moved_list); + + } + + monitored_bodies.clear(); + + _set_space(p_space); +} + + +void AreaSW::set_monitor_callback(ObjectID p_id, const StringName& p_method) { + + + if (p_id==monitor_callback_id) { + monitor_callback_method=p_method; + return; + } + + _unregister_shapes(); + + monitor_callback_id=p_id; + monitor_callback_method=p_method; + + monitored_bodies.clear(); + + _shape_changed(); + +} + + + +void AreaSW::set_space_override_mode(PhysicsServer::AreaSpaceOverrideMode p_mode) { + bool do_override=p_mode!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED; + if (do_override==(space_override_mode!=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED)) + return; + _unregister_shapes(); + space_override_mode=p_mode; + _shape_changed(); +} + +void AreaSW::set_param(PhysicsServer::AreaParameter p_param, const Variant& p_value) { + + switch(p_param) { + case PhysicsServer::AREA_PARAM_GRAVITY: gravity=p_value; ; break; + case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR: gravity_vector=p_value; ; break; + case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT: gravity_is_point=p_value; ; break; + case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: point_attenuation=p_value; ; break; + case PhysicsServer::AREA_PARAM_DENSITY: density=p_value; ; break; + case PhysicsServer::AREA_PARAM_PRIORITY: priority=p_value; ; break; + } + + +} + +Variant AreaSW::get_param(PhysicsServer::AreaParameter p_param) const { + + + switch(p_param) { + case PhysicsServer::AREA_PARAM_GRAVITY: return gravity; + case PhysicsServer::AREA_PARAM_GRAVITY_VECTOR: return gravity_vector; + case PhysicsServer::AREA_PARAM_GRAVITY_IS_POINT: return gravity_is_point; + case PhysicsServer::AREA_PARAM_GRAVITY_POINT_ATTENUATION: return point_attenuation; + case PhysicsServer::AREA_PARAM_DENSITY: return density; + case PhysicsServer::AREA_PARAM_PRIORITY: return priority; + } + + return Variant(); +} + + +void AreaSW::_queue_monitor_update() { + + ERR_FAIL_COND(!get_space()); + + if (!monitor_query_list.in_list()) + get_space()->area_add_to_monitor_query_list(&monitor_query_list); + + +} + +void AreaSW::call_queries() { + + if (monitor_callback_id && !monitored_bodies.empty()) { + + Variant res[5]; + Variant *resptr[5]; + for(int i=0;i<5;i++) + resptr[i]=&res[i]; + + Object *obj = ObjectDB::get_instance(monitor_callback_id); + if (!obj) { + monitored_bodies.clear(); + monitor_callback_id=0; + return; + } + + + + for (Map<BodyKey,BodyState>::Element *E=monitored_bodies.front();E;E=E->next()) { + + if (E->get().state==0) + continue; //nothing happened + + res[0]=E->get().state>0 ? PhysicsServer::AREA_BODY_ADDED : PhysicsServer::AREA_BODY_REMOVED; + res[1]=E->key().rid; + res[2]=E->key().instance_id; + res[3]=E->key().body_shape; + res[4]=E->key().area_shape; + + Variant::CallError ce; + obj->call(monitor_callback_method,(const Variant**)resptr,5,ce); + } + } + + monitored_bodies.clear(); + + //get_space()->area_remove_from_monitor_query_list(&monitor_query_list); + +} + +AreaSW::AreaSW() : CollisionObjectSW(TYPE_AREA), monitor_query_list(this), moved_list(this) { + + _set_static(true); //areas are never active + space_override_mode=PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED; + gravity=9.80665; + gravity_vector=Vector3(0,-1,0); + gravity_is_point=false; + point_attenuation=1; + density=0.1; + priority=0; + + +} + +AreaSW::~AreaSW() { + + +} + diff --git a/servers/physics/area_sw.h b/servers/physics/area_sw.h new file mode 100644 index 0000000000..3e39dc3bb6 --- /dev/null +++ b/servers/physics/area_sw.h @@ -0,0 +1,172 @@ +/*************************************************************************/ +/* area_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef AREA_SW_H +#define AREA_SW_H + +#include "servers/physics_server.h" +#include "collision_object_sw.h" +#include "self_list.h" +//#include "servers/physics/query_sw.h" + +class SpaceSW; +class BodySW; +class ConstraintSW; + +class AreaSW : public CollisionObjectSW{ + + + PhysicsServer::AreaSpaceOverrideMode space_override_mode; + float gravity; + Vector3 gravity_vector; + bool gravity_is_point; + float point_attenuation; + float density; + int priority; + + ObjectID monitor_callback_id; + StringName monitor_callback_method; + + SelfList<AreaSW> monitor_query_list; + SelfList<AreaSW> moved_list; + + struct BodyKey { + + RID rid; + ObjectID instance_id; + uint32_t body_shape; + uint32_t area_shape; + + _FORCE_INLINE_ bool operator<( const BodyKey& p_key) const { + + if (rid==p_key.rid) { + + if (body_shape==p_key.body_shape) { + + return area_shape < p_key.area_shape; + } else + return body_shape < p_key.area_shape; + } else + return rid < p_key.rid; + + } + + _FORCE_INLINE_ BodyKey() {} + BodyKey(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape); + }; + + struct BodyState { + + int state; + _FORCE_INLINE_ void inc() { state++; } + _FORCE_INLINE_ void dec() { state--; } + _FORCE_INLINE_ BodyState() { state=0; } + }; + + Map<BodyKey,BodyState> monitored_bodies; + + //virtual void shape_changed_notify(ShapeSW *p_shape); + //virtual void shape_deleted_notify(ShapeSW *p_shape); + + Set<ConstraintSW*> constraints; + + + virtual void _shapes_changed(); + void _queue_monitor_update(); + +public: + + //_FORCE_INLINE_ const Transform& get_inverse_transform() const { return inverse_transform; } + //_FORCE_INLINE_ SpaceSW* get_owner() { return owner; } + + void set_monitor_callback(ObjectID p_id, const StringName& p_method); + _FORCE_INLINE_ bool has_monitor_callback() const { return monitor_callback_id; } + + _FORCE_INLINE_ void add_body_to_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape); + _FORCE_INLINE_ void remove_body_from_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape); + + void set_param(PhysicsServer::AreaParameter p_param, const Variant& p_value); + Variant get_param(PhysicsServer::AreaParameter p_param) const; + + void set_space_override_mode(PhysicsServer::AreaSpaceOverrideMode p_mode); + PhysicsServer::AreaSpaceOverrideMode get_space_override_mode() const { return space_override_mode; } + + _FORCE_INLINE_ void set_gravity(float p_gravity) { gravity=p_gravity; } + _FORCE_INLINE_ float get_gravity() const { return gravity; } + + _FORCE_INLINE_ void set_gravity_vector(const Vector3& p_gravity) { gravity_vector=p_gravity; } + _FORCE_INLINE_ Vector3 get_gravity_vector() const { return gravity_vector; } + + _FORCE_INLINE_ void set_gravity_as_point(bool p_enable) { gravity_is_point=p_enable; } + _FORCE_INLINE_ bool is_gravity_point() const { return gravity_is_point; } + + _FORCE_INLINE_ void set_point_attenuation(float p_point_attenuation) { point_attenuation=p_point_attenuation; } + _FORCE_INLINE_ float get_point_attenuation() const { return point_attenuation; } + + _FORCE_INLINE_ void set_density(float p_density) { density=p_density; } + _FORCE_INLINE_ float get_density() const { return density; } + + _FORCE_INLINE_ void set_priority(int p_priority) { priority=p_priority; } + _FORCE_INLINE_ int get_priority() const { return priority; } + + _FORCE_INLINE_ void add_constraint( ConstraintSW* p_constraint) { constraints.insert(p_constraint); } + _FORCE_INLINE_ void remove_constraint( ConstraintSW* p_constraint) { constraints.erase(p_constraint); } + _FORCE_INLINE_ const Set<ConstraintSW*>& get_constraints() const { return constraints; } + + void set_transform(const Transform& p_transform); + + void set_space(SpaceSW *p_space); + + + void call_queries(); + + AreaSW(); + ~AreaSW(); +}; + +void AreaSW::add_body_to_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { + + BodyKey bk(p_body,p_body_shape,p_area_shape); + monitored_bodies[bk].inc(); + if (!monitor_query_list.in_list()) + _queue_monitor_update(); +} +void AreaSW::remove_body_from_query(BodySW *p_body, uint32_t p_body_shape,uint32_t p_area_shape) { + + BodyKey bk(p_body,p_body_shape,p_area_shape); + monitored_bodies[bk].dec(); + if (!monitor_query_list.in_list()) + _queue_monitor_update(); +} + + + + + + +#endif // AREA__SW_H diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp new file mode 100644 index 0000000000..06ae34098c --- /dev/null +++ b/servers/physics/body_pair_sw.cpp @@ -0,0 +1,442 @@ +/*************************************************************************/ +/* body_pair_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "body_pair_sw.h" +#include "collision_solver_sw.h" +#include "space_sw.h" + + +/* +#define NO_ACCUMULATE_IMPULSES +#define NO_SPLIT_IMPULSES + +#define NO_FRICTION +*/ + +#define NO_TANGENTIALS +/* BODY PAIR */ + + +//#define ALLOWED_PENETRATION 0.01 +#define RELAXATION_TIMESTEPS 3 +#define MIN_VELOCITY 0.0001 + +void BodyPairSW::_contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata) { + + BodyPairSW* pair = (BodyPairSW*)p_userdata; + pair->contact_added_callback(p_point_A,p_point_B); + +} + +void BodyPairSW::contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B) { + + // check if we already have the contact + + //Vector3 local_A = A->get_inv_transform().xform(p_point_A); + //Vector3 local_B = B->get_inv_transform().xform(p_point_B); + + Vector3 local_A = A->get_inv_transform().basis.xform(p_point_A); + Vector3 local_B = B->get_inv_transform().basis.xform(p_point_B-offset_B); + + + + int new_index = contact_count; + + ERR_FAIL_COND( new_index >= (MAX_CONTACTS+1) ); + + Contact contact; + + contact.acc_normal_impulse=0; + contact.acc_bias_impulse=0; + contact.acc_tangent_impulse=Vector3(); + contact.local_A=local_A; + contact.local_B=local_B; + contact.normal=(p_point_A-p_point_B).normalized(); + + + + // attempt to determine if the contact will be reused + real_t contact_recycle_radius=space->get_contact_recycle_radius(); + + for (int i=0;i<contact_count;i++) { + + Contact& c = contacts[i]; + if ( + c.local_A.distance_squared_to( local_A ) < (contact_recycle_radius*contact_recycle_radius) && + c.local_B.distance_squared_to( local_B ) < (contact_recycle_radius*contact_recycle_radius) ) { + + contact.acc_normal_impulse=c.acc_normal_impulse; + contact.acc_bias_impulse=c.acc_bias_impulse; + contact.acc_tangent_impulse=c.acc_tangent_impulse; + new_index=i; + break; + } + } + + // figure out if the contact amount must be reduced to fit the new contact + + if (new_index == MAX_CONTACTS) { + + // remove the contact with the minimum depth + + int least_deep=-1; + float min_depth=1e10; + + for (int i=0;i<=contact_count;i++) { + + Contact& c = (i==contact_count)?contact:contacts[i]; + Vector3 global_A = A->get_transform().basis.xform(c.local_A); + Vector3 global_B = B->get_transform().basis.xform(c.local_B)+offset_B; + + Vector3 axis = global_A - global_B; + float depth = axis.dot( c.normal ); + + if (depth<min_depth) { + + min_depth=depth; + least_deep=i; + } + } + + ERR_FAIL_COND(least_deep==-1); + + if (least_deep < contact_count) { //replace the last deep contact by the new one + + contacts[least_deep]=contact; + } + + return; + } + + contacts[new_index]=contact; + + if (new_index==contact_count) { + + contact_count++; + } + +} + +void BodyPairSW::validate_contacts() { + + //make sure to erase contacts that are no longer valid + + real_t contact_max_separation=space->get_contact_max_separation(); + for (int i=0;i<contact_count;i++) { + + Contact& c = contacts[i]; + + Vector3 global_A = A->get_transform().basis.xform(c.local_A); + Vector3 global_B = B->get_transform().basis.xform(c.local_B)+offset_B; + Vector3 axis = global_A - global_B; + float depth = axis.dot( c.normal ); + + if (depth < -contact_max_separation || (global_B + c.normal * depth - global_A).length() > contact_max_separation) { + // contact no longer needed, remove + + + if ((i+1) < contact_count) { + // swap with the last one + SWAP( contacts[i], contacts[ contact_count-1 ] ); + + } + + i--; + contact_count--; + } + } +} + +bool BodyPairSW::setup(float p_step) { + + + offset_B = B->get_transform().get_origin() - A->get_transform().get_origin(); + + validate_contacts(); + + Vector3 offset_A = A->get_transform().get_origin(); + Transform xform_Au = Transform(A->get_transform().basis,Vector3()); + Transform xform_A = xform_Au * A->get_shape_transform(shape_A); + + Transform xform_Bu = B->get_transform(); + xform_Bu.origin-=offset_A; + Transform xform_B = xform_Bu * B->get_shape_transform(shape_B); + + ShapeSW *shape_A_ptr=A->get_shape(shape_A); + ShapeSW *shape_B_ptr=B->get_shape(shape_B); + + bool collided = CollisionSolverSW::solve_static(shape_A_ptr,xform_A,shape_B_ptr,xform_B,_contact_added_callback,this,&sep_axis); + this->collided=collided; + + if (!collided) + return false; + + + //cannot collide + if (A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=PhysicsServer::BODY_MODE_STATIC_ACTIVE && B->get_mode()<=PhysicsServer::BODY_MODE_STATIC_ACTIVE)) { + return false; + } + + real_t max_penetration = space->get_contact_max_allowed_penetration(); + + float bias = 0.3f; + + if (shape_A_ptr->get_custom_bias() || shape_B_ptr->get_custom_bias()) { + + if (shape_A_ptr->get_custom_bias()==0) + bias=shape_B_ptr->get_custom_bias(); + else if (shape_B_ptr->get_custom_bias()==0) + bias=shape_A_ptr->get_custom_bias(); + else + bias=(shape_B_ptr->get_custom_bias()+shape_A_ptr->get_custom_bias())*0.5; + } + + + real_t inv_dt = 1.0/p_step; + + for(int i=0;i<contact_count;i++) { + + Contact &c = contacts[i]; + c.active=false; + + Vector3 global_A = xform_Au.xform(c.local_A); + Vector3 global_B = xform_Bu.xform(c.local_B); + + real_t depth = c.normal.dot(global_A - global_B); + + if (depth<=0) { + c.active=false; + continue; + } + + c.active=true; + + int gather_A = A->can_report_contacts(); + int gather_B = B->can_report_contacts(); + + c.rA = global_A; + c.rB = global_B-offset_B; + + // contact query reporting... +#if 0 + if (A->get_body_type() == PhysicsServer::BODY_CHARACTER) + static_cast<CharacterBodySW*>(A)->report_character_contact( global_A, global_B, B ); + if (B->get_body_type() == PhysicsServer::BODY_CHARACTER) + static_cast<CharacterBodySW*>(B)->report_character_contact( global_B, global_A, A ); + if (A->has_contact_query()) + A->report_contact( global_A, global_B, B ); + if (B->has_contact_query()) + B->report_contact( global_B, global_A, A ); +#endif + + if (A->can_report_contacts()) { + Vector3 crB = A->get_angular_velocity().cross( c.rA ) + A->get_linear_velocity(); + A->add_contact(global_A,-c.normal,depth,shape_A,global_B,shape_B,B->get_instance_id(),B->get_self(),crB); + } + + if (B->can_report_contacts()) { + Vector3 crA = A->get_angular_velocity().cross( c.rB ) + A->get_linear_velocity(); + B->add_contact(global_B,c.normal,depth,shape_B,global_A,shape_A,A->get_instance_id(),A->get_self(),crA); + } + + c.active=true; + + // Precompute normal mass, tangent mass, and bias. + Vector3 inertia_A = A->get_inv_inertia_tensor().xform( c.rA.cross( c.normal ) ); + Vector3 inertia_B = B->get_inv_inertia_tensor().xform( c.rB.cross( c.normal ) ); + real_t kNormal = A->get_inv_mass() + B->get_inv_mass(); + kNormal += c.normal.dot( inertia_A.cross(c.rA ) ) + c.normal.dot( inertia_B.cross( c.rB )); + c.mass_normal = 1.0f / kNormal; + +#if 1 + c.bias = -bias * inv_dt * MIN(0.0f, -depth + max_penetration); + +#else + if (depth > max_penetration) { + c.bias = (depth - max_penetration) * (1.0/(p_step*(1.0/RELAXATION_TIMESTEPS))); + } else { + float approach = -0.1f * (depth - max_penetration) / (CMP_EPSILON + max_penetration); + approach = CLAMP( approach, CMP_EPSILON, 1.0 ); + c.bias = approach * (depth - max_penetration) * (1.0/p_step); + } +#endif + c.depth=depth; + + Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse; + A->apply_impulse( c.rA, -j_vec ); + B->apply_impulse( c.rB, j_vec ); + c.acc_bias_impulse=0; + Vector3 jb_vec = c.normal * c.acc_bias_impulse; + A->apply_bias_impulse( c.rA, -jb_vec ); + B->apply_bias_impulse( c.rB, jb_vec ); + + } + + return true; +} + +void BodyPairSW::solve(float p_step) { + + if (!collided) + return; + + + for(int i=0;i<contact_count;i++) { + + Contact &c = contacts[i]; + if (!c.active) + continue; + + c.active=false; //try to deactivate, will activate itself if still needed + + //bias impule + + Vector3 crbA = A->get_biased_angular_velocity().cross( c.rA ); + Vector3 crbB = B->get_biased_angular_velocity().cross( c.rB ); + Vector3 dbv = B->get_biased_linear_velocity() + crbB - A->get_biased_linear_velocity() - crbA; + + real_t vbn = dbv.dot(c.normal); + + if (Math::abs(-vbn+c.bias)>MIN_VELOCITY) { + + real_t jbn = (-vbn + c.bias)*c.mass_normal; + real_t jbnOld = c.acc_bias_impulse; + c.acc_bias_impulse = MAX(jbnOld + jbn, 0.0f); + + Vector3 jb = c.normal * (c.acc_bias_impulse - jbnOld); + + + A->apply_bias_impulse(c.rA,-jb); + B->apply_bias_impulse(c.rB, jb); + + c.active=true; + } + + + Vector3 crA = A->get_angular_velocity().cross( c.rA ); + Vector3 crB = B->get_angular_velocity().cross( c.rB ); + Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA; + + //normal impule + real_t vn = dv.dot(c.normal); + + if (Math::abs(vn)>MIN_VELOCITY) { + + real_t bounce=0; + real_t jn = (-bounce -vn)*c.mass_normal; + real_t jnOld = c.acc_normal_impulse; + c.acc_normal_impulse = MAX(jnOld + jn, 0.0f); + + + Vector3 j =c.normal * (c.acc_normal_impulse - jnOld); + + + A->apply_impulse(c.rA,-j); + B->apply_impulse(c.rB, j); + + c.active=true; + } + + //friction impule + + real_t friction = A->get_friction() * B->get_friction(); + + Vector3 lvA = A->get_linear_velocity() + A->get_angular_velocity().cross( c.rA ); + Vector3 lvB = B->get_linear_velocity() + B->get_angular_velocity().cross( c.rB ); + + Vector3 dtv = lvB - lvA; + real_t tn = c.normal.dot(dtv); + + // tangential velocity + Vector3 tv = dtv - c.normal * tn; + real_t tvl = tv.length(); + + if (tvl > MIN_VELOCITY) { + + tv /= tvl; + + Vector3 temp1 = A->get_inv_inertia_tensor().xform( c.rA.cross( tv ) ); + Vector3 temp2 = B->get_inv_inertia_tensor().xform( c.rB.cross( tv ) ); + + real_t t = -tvl / + (A->get_inv_mass() + B->get_inv_mass() + tv.dot(temp1.cross(c.rA) + temp2.cross(c.rB))); + + Vector3 jt = t * tv; + + + Vector3 jtOld = c.acc_tangent_impulse; + c.acc_tangent_impulse += jt; + + real_t fi_len = c.acc_tangent_impulse.length(); + real_t jtMax = c.acc_normal_impulse * friction; + + if (fi_len > CMP_EPSILON && fi_len > jtMax) { + + c.acc_tangent_impulse*=jtMax / fi_len; + } + + jt = c.acc_tangent_impulse - jtOld; + + + A->apply_impulse( c.rA, -jt ); + B->apply_impulse( c.rB, jt ); + + c.active=true; + + } + + + } + +} + + + + + +BodyPairSW::BodyPairSW(BodySW *p_A, int p_shape_A,BodySW *p_B, int p_shape_B) : ConstraintSW(_arr,2) { + + A=p_A; + B=p_B; + shape_A=p_shape_A; + shape_B=p_shape_B; + space=A->get_space(); + A->add_constraint(this,0); + B->add_constraint(this,1); + contact_count=0; + collided=false; + +} + + +BodyPairSW::~BodyPairSW() { + + A->remove_constraint(this); + B->remove_constraint(this); + +} diff --git a/servers/physics/body_pair_sw.h b/servers/physics/body_pair_sw.h new file mode 100644 index 0000000000..ad66227b36 --- /dev/null +++ b/servers/physics/body_pair_sw.h @@ -0,0 +1,97 @@ +/*************************************************************************/ +/* body_pair_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef BODY_PAIR_SW_H +#define BODY_PAIR_SW_H + +#include "body_sw.h" +#include "constraint_sw.h" + +class BodyPairSW : public ConstraintSW { + enum { + + MAX_CONTACTS=4 + }; + + union { + struct { + BodySW *A; + BodySW *B; + }; + + BodySW *_arr[2]; + }; + + int shape_A; + int shape_B; + + + struct Contact { + + Vector3 position; + Vector3 normal; + Vector3 local_A, local_B; + real_t acc_normal_impulse; // accumulated normal impulse (Pn) + Vector3 acc_tangent_impulse; // accumulated tangent impulse (Pt) + real_t acc_bias_impulse; // accumulated normal impulse for position bias (Pnb) + real_t mass_normal; + real_t bias; + + real_t depth; + bool active; + Vector3 rA,rB; + }; + + Vector3 offset_B; //use local A coordinates to avoid numerical issues on collision detection + + Vector3 sep_axis; + Contact contacts[MAX_CONTACTS]; + int contact_count; + bool collided; + int cc; + + + static void _contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata); + + void contact_added_callback(const Vector3& p_point_A,const Vector3& p_point_B); + + void validate_contacts(); + + SpaceSW *space; + +public: + + bool setup(float p_step); + void solve(float p_step); + + BodyPairSW(BodySW *p_A, int p_shape_A,BodySW *p_B, int p_shape_B); + ~BodyPairSW(); + +}; + +#endif // BODY_PAIR__SW_H diff --git a/servers/physics/body_sw.cpp b/servers/physics/body_sw.cpp new file mode 100644 index 0000000000..b926a93773 --- /dev/null +++ b/servers/physics/body_sw.cpp @@ -0,0 +1,631 @@ +/*************************************************************************/ +/* body_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "body_sw.h"
+#include "space_sw.h"
+#include "area_sw.h"
+
+void BodySW::_update_inertia() {
+
+ if (get_space() && !inertia_update_list.in_list())
+ get_space()->body_add_to_inertia_update_list(&inertia_update_list);
+
+}
+
+
+void BodySW::_update_inertia_tensor() {
+
+ Matrix3 tb = get_transform().basis;
+ tb.scale(_inv_inertia);
+ _inv_inertia_tensor = tb * get_transform().basis.transposed();
+
+}
+
+void BodySW::update_inertias() {
+
+ //update shapes and motions
+
+ switch(mode) {
+
+ case PhysicsServer::BODY_MODE_RIGID: {
+
+ //update tensor for allshapes, not the best way but should be somehow OK. (inspired from bullet)
+ float total_area=0;
+
+ for (int i=0;i<get_shape_count();i++) {
+
+ total_area+=get_shape_aabb(i).get_area();
+ }
+
+ Vector3 _inertia;
+
+
+ for (int i=0;i<get_shape_count();i++) {
+
+ const ShapeSW* shape=get_shape(i);
+
+ float area=get_shape_aabb(i).get_area();
+
+ float mass = area * this->mass / total_area;
+
+ _inertia += shape->get_moment_of_inertia(mass) + mass * get_shape_transform(i).get_origin();
+
+ }
+
+ if (_inertia!=Vector3())
+ _inv_inertia=_inertia.inverse();
+ else
+ _inv_inertia=Vector3();
+
+ if (mass)
+ _inv_mass=1.0/mass;
+ else
+ _inv_mass=0;
+
+ } break;
+
+ case PhysicsServer::BODY_MODE_STATIC_ACTIVE:
+ case PhysicsServer::BODY_MODE_STATIC: {
+
+ _inv_inertia=Vector3();
+ _inv_mass=0;
+ } break;
+ case PhysicsServer::BODY_MODE_CHARACTER: {
+
+ _inv_inertia=Vector3();
+ _inv_mass=1.0/mass;
+
+ } break;
+ }
+ _update_inertia_tensor();
+
+ //_update_shapes();
+
+}
+
+
+
+void BodySW::set_active(bool p_active) {
+
+ if (active==p_active)
+ return;
+
+ active=p_active;
+ if (!p_active) {
+ if (get_space())
+ get_space()->body_remove_from_active_list(&active_list);
+ } else {
+ if (mode==PhysicsServer::BODY_MODE_STATIC)
+ return; //static bodies can't become active
+ if (get_space())
+ get_space()->body_add_to_active_list(&active_list);
+
+ //still_time=0;
+ }
+/*
+ if (!space)
+ return;
+
+ for(int i=0;i<get_shape_count();i++) {
+ Shape &s=shapes[i];
+ if (s.bpid>0) {
+ get_space()->get_broadphase()->set_active(s.bpid,active);
+ }
+ }
+*/
+}
+
+
+
+void BodySW::set_param(PhysicsServer::BodyParameter p_param, float p_value) {
+
+ switch(p_param) {
+ case PhysicsServer::BODY_PARAM_BOUNCE: {
+
+ bounce=p_value;
+ } break;
+ case PhysicsServer::BODY_PARAM_FRICTION: {
+
+ friction=p_value;
+ } break;
+ case PhysicsServer::BODY_PARAM_MASS: {
+ ERR_FAIL_COND(p_value<=0);
+ mass=p_value;
+ _update_inertia();
+
+ } break;
+ default:{}
+ }
+}
+
+float BodySW::get_param(PhysicsServer::BodyParameter p_param) const {
+
+ switch(p_param) {
+ case PhysicsServer::BODY_PARAM_BOUNCE: {
+
+ return bounce;
+ } break;
+ case PhysicsServer::BODY_PARAM_FRICTION: {
+
+ return friction;
+ } break;
+ case PhysicsServer::BODY_PARAM_MASS: {
+ return mass;
+ } break;
+ default:{}
+ }
+
+ return 0;
+}
+
+void BodySW::set_mode(PhysicsServer::BodyMode p_mode) {
+
+ mode=p_mode;
+
+ switch(p_mode) {
+ //CLEAR UP EVERYTHING IN CASE IT NOT WORKS!
+ case PhysicsServer::BODY_MODE_STATIC:
+ case PhysicsServer::BODY_MODE_STATIC_ACTIVE: {
+
+ _set_inv_transform(get_transform().affine_inverse());
+ _inv_mass=0;
+ _set_static(p_mode==PhysicsServer::BODY_MODE_STATIC);
+ set_active(p_mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE);
+ linear_velocity=Vector3();
+ angular_velocity=Vector3();
+ } break;
+ case PhysicsServer::BODY_MODE_RIGID: {
+
+ _inv_mass=mass>0?(1.0/mass):0;
+ _set_static(false);
+ simulated_motion=false; //jic
+
+ } break;
+ case PhysicsServer::BODY_MODE_CHARACTER: {
+
+ _inv_mass=mass>0?(1.0/mass):0;
+ _set_static(false);
+ simulated_motion=false; //jic
+ } break;
+ }
+
+ _update_inertia();
+ //if (get_space())
+// _update_queries();
+
+}
+PhysicsServer::BodyMode BodySW::get_mode() const {
+
+ return mode;
+}
+
+void BodySW::_shapes_changed() {
+
+ _update_inertia();
+}
+
+void BodySW::set_state(PhysicsServer::BodyState p_state, const Variant& p_variant) {
+
+ switch(p_state) {
+ case PhysicsServer::BODY_STATE_TRANSFORM: {
+
+
+ if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE) {
+ _set_transform(p_variant);
+ _set_inv_transform(get_transform().affine_inverse());
+ wakeup_neighbours();
+ } else {
+ Transform t = p_variant;
+ t.orthonormalize();
+ _set_transform(t);
+ _set_inv_transform(get_transform().inverse());
+
+ }
+
+ } break;
+ case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: {
+
+ //if (mode==PhysicsServer::BODY_MODE_STATIC)
+ // break;
+ linear_velocity=p_variant;
+ } break;
+ case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: {
+ //if (mode!=PhysicsServer::BODY_MODE_RIGID)
+ // break;
+ angular_velocity=p_variant;
+
+ } break;
+ case PhysicsServer::BODY_STATE_SLEEPING: {
+ //?
+ if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE)
+ break;
+ bool do_sleep=p_variant;
+ if (do_sleep) {
+ linear_velocity=Vector3();
+ //biased_linear_velocity=Vector3();
+ angular_velocity=Vector3();
+ //biased_angular_velocity=Vector3();
+ set_active(false);
+ } else {
+ if (mode!=PhysicsServer::BODY_MODE_STATIC)
+ set_active(true);
+ }
+ } break;
+ case PhysicsServer::BODY_STATE_CAN_SLEEP: {
+ can_sleep=p_variant;
+ if (mode==PhysicsServer::BODY_MODE_RIGID && !active && !can_sleep)
+ set_active(true);
+
+ } break;
+ }
+
+}
+Variant BodySW::get_state(PhysicsServer::BodyState p_state) const {
+
+ switch(p_state) {
+ case PhysicsServer::BODY_STATE_TRANSFORM: {
+ return get_transform();
+ } break;
+ case PhysicsServer::BODY_STATE_LINEAR_VELOCITY: {
+ return linear_velocity;
+ } break;
+ case PhysicsServer::BODY_STATE_ANGULAR_VELOCITY: {
+ return angular_velocity;
+ } break;
+ case PhysicsServer::BODY_STATE_SLEEPING: {
+ return !is_active();
+ } break;
+ case PhysicsServer::BODY_STATE_CAN_SLEEP: {
+ return can_sleep;
+ } break;
+ }
+
+ return Variant();
+}
+
+
+void BodySW::set_space(SpaceSW *p_space){
+
+ if (get_space()) {
+
+ if (inertia_update_list.in_list())
+ get_space()->body_remove_from_inertia_update_list(&inertia_update_list);
+ if (active_list.in_list())
+ get_space()->body_remove_from_active_list(&active_list);
+ if (direct_state_query_list.in_list())
+ get_space()->body_remove_from_state_query_list(&direct_state_query_list);
+
+ }
+
+ _set_space(p_space);
+
+ if (get_space()) {
+
+ _update_inertia();
+ if (active)
+ get_space()->body_add_to_active_list(&active_list);
+// _update_queries();
+ //if (is_active()) {
+ // active=false;
+ // set_active(true);
+ //}
+
+ }
+
+}
+
+void BodySW::_compute_area_gravity(const AreaSW *p_area) {
+
+ if (p_area->is_gravity_point()) {
+
+ gravity = (p_area->get_gravity_vector() - get_transform().get_origin()).normalized() * p_area->get_gravity();
+
+ } else {
+ gravity = p_area->get_gravity_vector() * p_area->get_gravity();
+ }
+}
+
+void BodySW::integrate_forces(real_t p_step) {
+
+
+ if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE)
+ return;
+
+ AreaSW *current_area = get_space()->get_default_area();
+ ERR_FAIL_COND(!current_area);
+
+ int prio = current_area->get_priority();
+ int ac = areas.size();
+ if (ac) {
+ const AreaCMP *aa = &areas[0];
+ for(int i=0;i<ac;i++) {
+ if (aa[i].area->get_priority() > prio) {
+ current_area=aa[i].area;
+ prio=current_area->get_priority();
+ }
+ }
+ }
+
+ _compute_area_gravity(current_area);
+ density=current_area->get_density();
+
+ if (!omit_force_integration) {
+ //overriden by direct state query
+
+ Vector3 force=gravity*mass;
+ force+=applied_force;
+ Vector3 torque=applied_torque;
+
+ real_t damp = 1.0 - p_step * density;
+
+ if (damp<0) // reached zero in the given time
+ damp=0;
+
+ real_t angular_damp = 1.0 - p_step * density * get_space()->get_body_angular_velocity_damp_ratio();
+
+ if (angular_damp<0) // reached zero in the given time
+ angular_damp=0;
+
+ linear_velocity*=damp;
+ angular_velocity*=angular_damp;
+
+ linear_velocity+=_inv_mass * force * p_step;
+ angular_velocity+=_inv_inertia_tensor.xform(torque)*p_step;
+ }
+
+ applied_force=Vector3();
+ applied_torque=Vector3();
+
+ //motion=linear_velocity*p_step;
+
+ biased_angular_velocity=Vector3();
+ biased_linear_velocity=Vector3();
+
+ if (continuous_cd) //shapes temporarily extend for raycast
+ _update_shapes_with_motion(linear_velocity*p_step);
+
+ current_area=NULL; // clear the area, so it is set in the next frame
+ contact_count=0;
+
+}
+
+void BodySW::integrate_velocities(real_t p_step) {
+
+ if (mode==PhysicsServer::BODY_MODE_STATIC)
+ return;
+
+ if (mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE) {
+ if (fi_callback)
+ get_space()->body_add_to_state_query_list(&direct_state_query_list);
+ return;
+ }
+
+ Vector3 total_angular_velocity = angular_velocity+biased_angular_velocity;
+
+
+
+ float ang_vel = total_angular_velocity.length();
+ Transform transform = get_transform();
+
+
+ if (ang_vel!=0.0) {
+ Vector3 ang_vel_axis = total_angular_velocity / ang_vel;
+ Matrix3 rot( ang_vel_axis, -ang_vel*p_step );
+ transform.basis = rot * transform.basis;
+ transform.orthonormalize();
+ }
+
+ Vector3 total_linear_velocity=linear_velocity+biased_linear_velocity;
+
+
+ transform.origin+=total_linear_velocity * p_step;
+
+ _set_transform(transform);
+ _set_inv_transform(get_transform().inverse());
+
+ _update_inertia_tensor();
+
+ if (fi_callback) {
+
+ get_space()->body_add_to_state_query_list(&direct_state_query_list);
+ }
+
+}
+
+
+void BodySW::simulate_motion(const Transform& p_xform,real_t p_step) {
+
+ Transform inv_xform = p_xform.affine_inverse();
+ if (!get_space()) {
+ _set_transform(p_xform);
+ _set_inv_transform(inv_xform);
+
+ return;
+ }
+
+ //compute a FAKE linear velocity - this is easy
+
+ linear_velocity=(p_xform.origin - get_transform().origin)/p_step;
+
+ //compute a FAKE angular velocity, not so easy
+ Matrix3 rot=get_transform().basis.orthonormalized().transposed() * p_xform.basis.orthonormalized();
+ Vector3 axis;
+ float angle;
+
+ rot.get_axis_and_angle(axis,angle);
+ axis.normalize();
+ angular_velocity=axis.normalized() * (angle/p_step);
+ linear_velocity = (p_xform.origin - get_transform().origin)/p_step;
+
+ if (!direct_state_query_list.in_list())// - callalways, so lv and av are cleared && (state_query || direct_state_query))
+ get_space()->body_add_to_state_query_list(&direct_state_query_list);
+ simulated_motion=true;
+ _set_transform(p_xform);
+
+
+}
+
+void BodySW::wakeup_neighbours() {
+
+ for(Map<ConstraintSW*,int>::Element *E=constraint_map.front();E;E=E->next()) {
+
+ const ConstraintSW *c=E->key();
+ BodySW **n = c->get_body_ptr();
+ int bc=c->get_body_count();
+
+ for(int i=0;i<bc;i++) {
+
+ if (i==E->get())
+ continue;
+ BodySW *b = n[i];
+ if (b->mode!=PhysicsServer::BODY_MODE_RIGID)
+ continue;
+
+ if (!b->is_active())
+ b->set_active(true);
+ }
+ }
+}
+
+void BodySW::call_queries() {
+
+
+ if (fi_callback) {
+
+ PhysicsDirectBodyStateSW *dbs = PhysicsDirectBodyStateSW::singleton;
+ dbs->body=this;
+
+ Variant v=dbs;
+
+ Object *obj = ObjectDB::get_instance(fi_callback->id);
+ if (!obj) {
+
+ set_force_integration_callback(0,StringName());
+ } else {
+ const Variant *vp[2]={&v,&fi_callback->udata};
+
+ Variant::CallError ce;
+ int argc=(fi_callback->udata.get_type()==Variant::NIL)?1:2;
+ obj->call(fi_callback->method,vp,argc,ce);
+ }
+
+
+ }
+
+ if (simulated_motion) {
+
+ // linear_velocity=Vector3();
+ // angular_velocity=0;
+ simulated_motion=false;
+ }
+}
+
+
+bool BodySW::sleep_test(real_t p_step) {
+
+ if (mode==PhysicsServer::BODY_MODE_STATIC || mode==PhysicsServer::BODY_MODE_STATIC_ACTIVE)
+ return true; //
+ else if (mode==PhysicsServer::BODY_MODE_CHARACTER)
+ return !active; // characters don't sleep unless asked to sleep
+ else if (!can_sleep)
+ return false;
+
+
+
+
+ if (Math::abs(angular_velocity.length())<get_space()->get_body_angular_velocity_sleep_treshold() && Math::abs(linear_velocity.length_squared()) < get_space()->get_body_linear_velocity_sleep_treshold()*get_space()->get_body_linear_velocity_sleep_treshold()) {
+
+ still_time+=p_step;
+
+ return still_time > get_space()->get_body_time_to_sleep();
+ } else {
+
+ still_time=0; //maybe this should be set to 0 on set_active?
+ return false;
+ }
+}
+
+
+void BodySW::set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata) {
+
+ if (fi_callback) {
+
+ memdelete(fi_callback);
+ fi_callback=NULL;
+ }
+
+
+ if (p_id!=0) {
+
+ fi_callback=memnew(ForceIntegrationCallback);
+ fi_callback->id=p_id;
+ fi_callback->method=p_method;
+ fi_callback->udata=p_udata;
+ }
+
+}
+
+BodySW::BodySW() : CollisionObjectSW(TYPE_BODY), active_list(this), inertia_update_list(this), direct_state_query_list(this) {
+
+
+ mode=PhysicsServer::BODY_MODE_RIGID;
+ active=true;
+
+ mass=1;
+// _inv_inertia=Transform();
+ _inv_mass=1;
+ bounce=0;
+ friction=1;
+ omit_force_integration=false;
+// applied_torque=0;
+ island_step=0;
+ island_next=NULL;
+ island_list_next=NULL;
+ _set_static(false);
+ density=0;
+ contact_count=0;
+ simulated_motion=false;
+ still_time=0;
+ continuous_cd=false;
+ can_sleep=false;
+ fi_callback=NULL;
+
+}
+
+BodySW::~BodySW() {
+
+ if (fi_callback)
+ memdelete(fi_callback);
+}
+
+PhysicsDirectBodyStateSW *PhysicsDirectBodyStateSW::singleton=NULL;
+
+PhysicsDirectSpaceState* PhysicsDirectBodyStateSW::get_space_state() {
+
+ return body->get_space()->get_direct_state();
+}
diff --git a/servers/physics/body_sw.h b/servers/physics/body_sw.h new file mode 100644 index 0000000000..9f0bbc00cf --- /dev/null +++ b/servers/physics/body_sw.h @@ -0,0 +1,348 @@ +/*************************************************************************/ +/* body_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef BODY_SW_H
+#define BODY_SW_H
+
+#include "collision_object_sw.h"
+#include "vset.h"
+#include "area_sw.h"
+
+class ConstraintSW;
+
+
+class BodySW : public CollisionObjectSW {
+
+
+ PhysicsServer::BodyMode mode;
+
+ Vector3 linear_velocity;
+ Vector3 angular_velocity;
+
+ Vector3 biased_linear_velocity;
+ Vector3 biased_angular_velocity;
+ real_t mass;
+ real_t bounce;
+ real_t friction;
+
+ real_t _inv_mass;
+ Vector3 _inv_inertia;
+ Matrix3 _inv_inertia_tensor;
+
+ Vector3 gravity;
+ real_t density;
+
+ real_t still_time;
+
+ Vector3 applied_force;
+ Vector3 applied_torque;
+
+ SelfList<BodySW> active_list;
+ SelfList<BodySW> inertia_update_list;
+ SelfList<BodySW> direct_state_query_list;
+
+ VSet<RID> exceptions;
+ bool omit_force_integration;
+ bool active;
+ bool simulated_motion;
+ bool continuous_cd;
+ bool can_sleep;
+ void _update_inertia();
+ virtual void _shapes_changed();
+
+ Map<ConstraintSW*,int> constraint_map;
+
+ struct AreaCMP {
+
+ AreaSW *area;
+ _FORCE_INLINE_ bool operator<(const AreaCMP& p_cmp) const { return area->get_self() < p_cmp.area->get_self() ; }
+ _FORCE_INLINE_ AreaCMP() {}
+ _FORCE_INLINE_ AreaCMP(AreaSW *p_area) { area=p_area;}
+ };
+
+
+ VSet<AreaCMP> areas;
+
+ struct Contact {
+
+
+ Vector3 local_pos;
+ Vector3 local_normal;
+ float depth;
+ int local_shape;
+ Vector3 collider_pos;
+ int collider_shape;
+ ObjectID collider_instance_id;
+ RID collider;
+ Vector3 collider_velocity_at_pos;
+ };
+
+ Vector<Contact> contacts; //no contacts by default
+ int contact_count;
+
+ struct ForceIntegrationCallback {
+
+ ObjectID id;
+ StringName method;
+ Variant udata;
+ };
+
+ ForceIntegrationCallback *fi_callback;
+
+
+ uint64_t island_step;
+ BodySW *island_next;
+ BodySW *island_list_next;
+
+ _FORCE_INLINE_ void _compute_area_gravity(const AreaSW *p_area);
+
+ _FORCE_INLINE_ void _update_inertia_tensor();
+
+friend class PhysicsDirectBodyStateSW; // i give up, too many functions to expose
+
+public:
+
+
+ void set_force_integration_callback(ObjectID p_id,const StringName& p_method,const Variant& p_udata=Variant());
+
+
+ _FORCE_INLINE_ void add_area(AreaSW *p_area) { areas.insert(AreaCMP(p_area)); }
+ _FORCE_INLINE_ void remove_area(AreaSW *p_area) { areas.erase(AreaCMP(p_area)); }
+
+ _FORCE_INLINE_ void set_max_contacts_reported(int p_size) { contacts.resize(p_size); contact_count=0; }
+ _FORCE_INLINE_ int get_max_contacts_reported() const { return contacts.size(); }
+
+ _FORCE_INLINE_ bool can_report_contacts() const { return !contacts.empty(); }
+ _FORCE_INLINE_ void add_contact(const Vector3& p_local_pos,const Vector3& p_local_normal, float p_depth, int p_local_shape, const Vector3& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector3& p_collider_velocity_at_pos);
+
+
+ _FORCE_INLINE_ void add_exception(const RID& p_exception) { exceptions.insert(p_exception);}
+ _FORCE_INLINE_ void remove_exception(const RID& p_exception) { exceptions.erase(p_exception);}
+ _FORCE_INLINE_ bool has_exception(const RID& p_exception) const { return exceptions.has(p_exception);}
+ _FORCE_INLINE_ const VSet<RID>& get_exceptions() const { return exceptions;}
+
+ _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; }
+ _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; }
+
+ _FORCE_INLINE_ BodySW* get_island_next() const { return island_next; }
+ _FORCE_INLINE_ void set_island_next(BodySW* p_next) { island_next=p_next; }
+
+ _FORCE_INLINE_ BodySW* get_island_list_next() const { return island_list_next; }
+ _FORCE_INLINE_ void set_island_list_next(BodySW* p_next) { island_list_next=p_next; }
+
+ _FORCE_INLINE_ void add_constraint(ConstraintSW* p_constraint, int p_pos) { constraint_map[p_constraint]=p_pos; }
+ _FORCE_INLINE_ void remove_constraint(ConstraintSW* p_constraint) { constraint_map.erase(p_constraint); }
+ const Map<ConstraintSW*,int>& get_constraint_map() const { return constraint_map; }
+
+ _FORCE_INLINE_ void set_omit_force_integration(bool p_omit_force_integration) { omit_force_integration=p_omit_force_integration; }
+ _FORCE_INLINE_ bool get_omit_force_integration() const { return omit_force_integration; }
+
+ _FORCE_INLINE_ void set_linear_velocity(const Vector3& p_velocity) {linear_velocity=p_velocity; }
+ _FORCE_INLINE_ Vector3 get_linear_velocity() const { return linear_velocity; }
+
+ _FORCE_INLINE_ void set_angular_velocity(const Vector3& p_velocity) { angular_velocity=p_velocity; }
+ _FORCE_INLINE_ Vector3 get_angular_velocity() const { return angular_velocity; }
+
+ _FORCE_INLINE_ const Vector3& get_biased_linear_velocity() const { return biased_linear_velocity; }
+ _FORCE_INLINE_ const Vector3& get_biased_angular_velocity() const { return biased_angular_velocity; }
+
+ _FORCE_INLINE_ void apply_impulse(const Vector3& p_pos, const Vector3& p_j) {
+
+ linear_velocity += p_j * _inv_mass;
+ angular_velocity += _inv_inertia_tensor.xform( p_pos.cross(p_j) );
+ }
+
+ _FORCE_INLINE_ void apply_bias_impulse(const Vector3& p_pos, const Vector3& p_j) {
+
+ biased_linear_velocity += p_j * _inv_mass;
+ biased_angular_velocity += _inv_inertia_tensor.xform( p_pos.cross(p_j) );
+ }
+
+ _FORCE_INLINE_ void apply_torque_impulse(const Vector3& p_j) {
+
+ angular_velocity += _inv_inertia_tensor.xform(p_j);
+ }
+
+ _FORCE_INLINE_ void add_force(const Vector3& p_force, const Vector3& p_pos) {
+
+ applied_force += p_force;
+ applied_torque += p_pos.cross(p_force);
+ }
+
+ void set_active(bool p_active);
+ _FORCE_INLINE_ bool is_active() const { return active; }
+
+ void set_param(PhysicsServer::BodyParameter p_param, float);
+ float get_param(PhysicsServer::BodyParameter p_param) const;
+
+ void set_mode(PhysicsServer::BodyMode p_mode);
+ PhysicsServer::BodyMode get_mode() const;
+
+ void set_state(PhysicsServer::BodyState p_state, const Variant& p_variant);
+ Variant get_state(PhysicsServer::BodyState p_state) const;
+
+ void set_applied_force(const Vector3& p_force) { applied_force=p_force; }
+ Vector3 get_applied_force() const { return applied_force; }
+
+ void set_applied_torque(const Vector3& p_torque) { applied_torque=p_torque; }
+ Vector3 get_applied_torque() const { return applied_torque; }
+
+ _FORCE_INLINE_ void set_continuous_collision_detection(bool p_enable) { continuous_cd=p_enable; }
+ _FORCE_INLINE_ bool is_continuous_collision_detection_enabled() const { return continuous_cd; }
+
+ void set_space(SpaceSW *p_space);
+
+ void update_inertias();
+
+ _FORCE_INLINE_ real_t get_inv_mass() const { return _inv_mass; }
+ _FORCE_INLINE_ Vector3 get_inv_inertia() const { return _inv_inertia; }
+ _FORCE_INLINE_ Matrix3 get_inv_inertia_tensor() const { return _inv_inertia_tensor; }
+ _FORCE_INLINE_ real_t get_friction() const { return friction; }
+ _FORCE_INLINE_ Vector3 get_gravity() const { return gravity; }
+ _FORCE_INLINE_ real_t get_density() const { return density; }
+ _FORCE_INLINE_ real_t get_bounce() const { return bounce; }
+
+ void integrate_forces(real_t p_step);
+ void integrate_velocities(real_t p_step);
+
+ void simulate_motion(const Transform& p_xform,real_t p_step);
+ void call_queries();
+ void wakeup_neighbours();
+
+ bool sleep_test(real_t p_step);
+
+ BodySW();
+ ~BodySW();
+
+};
+
+
+//add contact inline
+
+void BodySW::add_contact(const Vector3& p_local_pos,const Vector3& p_local_normal, float p_depth, int p_local_shape, const Vector3& p_collider_pos, int p_collider_shape, ObjectID p_collider_instance_id, const RID& p_collider,const Vector3& p_collider_velocity_at_pos) {
+
+ int c_max=contacts.size();
+
+ if (c_max==0)
+ return;
+
+ Contact *c = &contacts[0];
+
+
+ int idx=-1;
+
+ if (contact_count<c_max) {
+ idx=contact_count++;
+ } else {
+
+ float least_depth=1e20;
+ int least_deep=-1;
+ for(int i=0;i<c_max;i++) {
+
+ if (i==0 || c[i].depth<least_depth) {
+ least_deep=i;
+ least_depth=c[i].depth;
+ }
+ }
+
+ if (least_deep>=0 && least_depth<p_depth) {
+
+ idx=least_deep;
+ }
+ if (idx==-1)
+ return; //none least deepe than this
+ }
+
+ c[idx].local_pos=p_local_pos;
+ c[idx].local_normal=p_local_normal;
+ c[idx].depth=p_depth;
+ c[idx].local_shape=p_local_shape;
+ c[idx].collider_pos=p_collider_pos;
+ c[idx].collider_shape=p_collider_shape;
+ c[idx].collider_instance_id=p_collider_instance_id;
+ c[idx].collider=p_collider;
+ c[idx].collider_velocity_at_pos=p_collider_velocity_at_pos;
+
+}
+
+
+class PhysicsDirectBodyStateSW : public PhysicsDirectBodyState {
+
+ OBJ_TYPE( PhysicsDirectBodyStateSW, PhysicsDirectBodyState );
+
+public:
+
+ static PhysicsDirectBodyStateSW *singleton;
+ BodySW *body;
+ real_t step;
+
+ virtual Vector3 get_total_gravity() const { return body->get_gravity(); } // get gravity vector working on this body space/area
+ virtual float get_total_density() const { return body->get_density(); } // get density of this body space/area
+
+ virtual float get_inverse_mass() const { return body->get_inv_mass(); } // get the mass
+ virtual Vector3 get_inverse_inertia() const { return body->get_inv_inertia(); } // get density of this body space
+ virtual Matrix3 get_inverse_inertia_tensor() const { return body->get_inv_inertia_tensor(); } // get density of this body space
+
+ virtual void set_linear_velocity(const Vector3& p_velocity) { body->set_linear_velocity(p_velocity); }
+ virtual Vector3 get_linear_velocity() const { return body->get_linear_velocity(); }
+
+ virtual void set_angular_velocity(const Vector3& p_velocity) { body->set_angular_velocity(p_velocity); }
+ virtual Vector3 get_angular_velocity() const { return body->get_angular_velocity(); }
+
+ virtual void set_transform(const Transform& p_transform) { body->set_state(PhysicsServer::BODY_STATE_TRANSFORM,p_transform); }
+ virtual Transform get_transform() const { return body->get_transform(); }
+
+ virtual void add_force(const Vector3& p_force, const Vector3& p_pos) { body->add_force(p_force,p_pos); }
+
+ virtual void set_sleep_state(bool p_enable) { body->set_active(!p_enable); }
+ virtual bool is_sleeping() const { return !body->is_active(); }
+
+ virtual int get_contact_count() const { return body->contact_count; }
+
+ virtual Vector3 get_contact_local_pos(int p_contact_idx) const {
+ ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3());
+ return body->contacts[p_contact_idx].local_pos;
+ }
+ virtual Vector3 get_contact_local_normal(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3()); return body->contacts[p_contact_idx].local_normal; }
+ virtual int get_contact_local_shape(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,-1); return body->contacts[p_contact_idx].local_shape; }
+
+ virtual RID get_contact_collider(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,RID()); return body->contacts[p_contact_idx].collider; }
+ virtual Vector3 get_contact_collider_pos(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3()); return body->contacts[p_contact_idx].collider_pos; }
+ virtual ObjectID get_contact_collider_id(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_instance_id; }
+ virtual int get_contact_collider_shape(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,0); return body->contacts[p_contact_idx].collider_shape; }
+ virtual Vector3 get_contact_collider_velocity_at_pos(int p_contact_idx) const { ERR_FAIL_INDEX_V(p_contact_idx,body->contact_count,Vector3()); return body->contacts[p_contact_idx].collider_velocity_at_pos; }
+
+ virtual PhysicsDirectSpaceState* get_space_state();
+
+
+ virtual real_t get_step() const { return step; }
+ PhysicsDirectBodyStateSW() { singleton=this; body=NULL; }
+};
+
+
+#endif // BODY__SW_H
diff --git a/servers/physics/broad_phase_basic.cpp b/servers/physics/broad_phase_basic.cpp new file mode 100644 index 0000000000..5db78e669f --- /dev/null +++ b/servers/physics/broad_phase_basic.cpp @@ -0,0 +1,216 @@ +/*************************************************************************/ +/* broad_phase_basic.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "broad_phase_basic.h" +#include "list.h" +#include "print_string.h" +BroadPhaseSW::ID BroadPhaseBasic::create(CollisionObjectSW *p_object_, int p_subindex) { + + if (p_object_==NULL) { + + ERR_FAIL_COND_V(p_object_==NULL,0); + } + + current++; + + Element e; + e.owner=p_object_; + e._static=false; + e.subindex=p_subindex; + + element_map[current]=e; + return current; +} + +void BroadPhaseBasic::move(ID p_id, const AABB& p_aabb) { + + Map<ID,Element>::Element *E=element_map.find(p_id); + ERR_FAIL_COND(!E); + E->get().aabb=p_aabb; + +} +void BroadPhaseBasic::set_static(ID p_id, bool p_static) { + + Map<ID,Element>::Element *E=element_map.find(p_id); + ERR_FAIL_COND(!E); + E->get()._static=p_static; + +} +void BroadPhaseBasic::remove(ID p_id) { + + Map<ID,Element>::Element *E=element_map.find(p_id); + ERR_FAIL_COND(!E); + List<PairKey> to_erase; + //unpair must be done immediately on removal to avoid potential invalid pointers + for (Map<PairKey,void*>::Element *F=pair_map.front();F;F=F->next()) { + + if (F->key().a==p_id || F->key().b==p_id) { + + if (unpair_callback) { + Element *elem_A=&element_map[F->key().a]; + Element *elem_B=&element_map[F->key().b]; + unpair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,F->get(),unpair_userdata); + } + to_erase.push_back(F->key()); + } + } + while(to_erase.size()) { + + pair_map.erase(to_erase.front()->get()); + to_erase.pop_front(); + } + element_map.erase(E); + +} + +CollisionObjectSW *BroadPhaseBasic::get_object(ID p_id) const { + + const Map<ID,Element>::Element *E=element_map.find(p_id); + ERR_FAIL_COND_V(!E,NULL); + return E->get().owner; + +} +bool BroadPhaseBasic::is_static(ID p_id) const { + + const Map<ID,Element>::Element *E=element_map.find(p_id); + ERR_FAIL_COND_V(!E,false); + return E->get()._static; + +} +int BroadPhaseBasic::get_subindex(ID p_id) const { + + const Map<ID,Element>::Element *E=element_map.find(p_id); + ERR_FAIL_COND_V(!E,-1); + return E->get().subindex; +} + +int BroadPhaseBasic::cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices) { + + int rc=0; + + for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) { + + const AABB aabb=E->get().aabb; + if (aabb.intersects_segment(p_from,p_to)) { + + p_results[rc]=E->get().owner; + p_result_indices[rc]=E->get().subindex; + rc++; + if (rc>=p_max_results) + break; + } + } + + return rc; + +} +int BroadPhaseBasic::cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices) { + + int rc=0; + + for (Map<ID,Element>::Element *E=element_map.front();E;E=E->next()) { + + const AABB aabb=E->get().aabb; + if (aabb.intersects(p_aabb)) { + + p_results[rc]=E->get().owner; + p_result_indices[rc]=E->get().subindex; + rc++; + if (rc>=p_max_results) + break; + } + } + + return rc; +} + +void BroadPhaseBasic::set_pair_callback(PairCallback p_pair_callback,void *p_userdata) { + + pair_userdata=p_userdata; + pair_callback=p_pair_callback; + +} +void BroadPhaseBasic::set_unpair_callback(UnpairCallback p_pair_callback,void *p_userdata) { + + unpair_userdata=p_userdata; + unpair_callback=p_pair_callback; + +} + +void BroadPhaseBasic::update() { + + // recompute pairs + for(Map<ID,Element>::Element *I=element_map.front();I;I=I->next()) { + + for(Map<ID,Element>::Element *J=I->next();J;J=J->next()) { + + Element *elem_A=&I->get(); + Element *elem_B=&J->get(); + + if (elem_A->owner == elem_B->owner) + continue; + + + bool pair_ok=elem_A->aabb.intersects( elem_B->aabb ) && (!elem_A->_static || !elem_B->_static ); + + PairKey key(I->key(),J->key()); + + Map<PairKey,void*>::Element *E=pair_map.find(key); + + if (!pair_ok && E) { + if (unpair_callback) + unpair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,E->get(),unpair_userdata); + pair_map.erase(key); + } + + if (pair_ok && !E) { + + void *data=NULL; + if (pair_callback) + data=pair_callback(elem_A->owner,elem_A->subindex,elem_B->owner,elem_B->subindex,unpair_userdata); + pair_map.insert(key,data); + } + } + } + +} + +BroadPhaseSW *BroadPhaseBasic::_create() { + + return memnew( BroadPhaseBasic ); +} + +BroadPhaseBasic::BroadPhaseBasic() { + + current=1; + unpair_callback=NULL; + unpair_userdata=NULL; + pair_callback=NULL; + pair_userdata=NULL; + +} diff --git a/servers/physics/broad_phase_basic.h b/servers/physics/broad_phase_basic.h new file mode 100644 index 0000000000..7b77466292 --- /dev/null +++ b/servers/physics/broad_phase_basic.h @@ -0,0 +1,101 @@ +/*************************************************************************/ +/* broad_phase_basic.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef BROAD_PHASE_BASIC_H +#define BROAD_PHASE_BASIC_H + +#include "broad_phase_sw.h" +#include "map.h" + +class BroadPhaseBasic : public BroadPhaseSW { + + struct Element { + + CollisionObjectSW *owner; + bool _static; + AABB aabb; + int subindex; + }; + + + Map<ID,Element> element_map; + + ID current; + + struct PairKey { + + union { + struct { + ID a; + ID b; + }; + uint64_t key; + }; + + _FORCE_INLINE_ bool operator<(const PairKey& p_key) const { + return key < p_key.key; + } + + PairKey() { key=0; } + PairKey(ID p_a, ID p_b) { if (p_a>p_b) { a=p_b; b=p_a; } else { a=p_a; b=p_b; }} + + }; + + Map<PairKey,void*> pair_map; + + + PairCallback pair_callback; + void *pair_userdata; + UnpairCallback unpair_callback; + void *unpair_userdata; + +public: + + // 0 is an invalid ID + virtual ID create(CollisionObjectSW *p_object_, int p_subindex=0); + virtual void move(ID p_id, const AABB& p_aabb); + virtual void set_static(ID p_id, bool p_static); + virtual void remove(ID p_id); + + virtual CollisionObjectSW *get_object(ID p_id) const; + virtual bool is_static(ID p_id) const; + virtual int get_subindex(ID p_id) const; + + virtual int cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL); + virtual int cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL); + + virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata); + virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata); + + virtual void update(); + + static BroadPhaseSW *_create(); + BroadPhaseBasic(); +}; + +#endif // BROAD_PHASE_BASIC_H diff --git a/servers/physics/broad_phase_octree.cpp b/servers/physics/broad_phase_octree.cpp new file mode 100644 index 0000000000..edf4aae2b2 --- /dev/null +++ b/servers/physics/broad_phase_octree.cpp @@ -0,0 +1,133 @@ +/*************************************************************************/ +/* broad_phase_octree.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "broad_phase_octree.h" +#include "collision_object_sw.h" + +ID BroadPhaseOctree::create(CollisionObjectSW *p_object, int p_subindex) { + + ID oid = octree.create(p_object,AABB(),p_subindex,false,1<<p_object->get_type(),0); + return oid; +} + +void BroadPhaseOctree::move(ID p_id, const AABB& p_aabb){ + + octree.move(p_id,p_aabb); +} + +void BroadPhaseOctree::set_static(ID p_id, bool p_static){ + + CollisionObjectSW *it = octree.get(p_id); + octree.set_pairable(p_id,p_static?false:true,1<<it->get_type(),p_static?0:0xFFFFF); //pair everything, don't care 1? + +} +void BroadPhaseOctree::remove(ID p_id){ + + octree.erase(p_id); +} + +CollisionObjectSW *BroadPhaseOctree::get_object(ID p_id) const{ + + CollisionObjectSW *it = octree.get(p_id); + ERR_FAIL_COND_V(!it,NULL); + return it; +} +bool BroadPhaseOctree::is_static(ID p_id) const{ + + return !octree.is_pairable(p_id); +} +int BroadPhaseOctree::get_subindex(ID p_id) const{ + + return octree.get_subindex(p_id); +} + +int BroadPhaseOctree::cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices){ + + return octree.cull_segment(p_from,p_to,p_results,p_max_results,p_result_indices); +} + +int BroadPhaseOctree::cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices) { + + return octree.cull_AABB(p_aabb,p_results,p_max_results,p_result_indices); + +} + + +void* BroadPhaseOctree::_pair_callback(void*self,OctreeElementID p_A, CollisionObjectSW*p_object_A,int subindex_A,OctreeElementID p_B, CollisionObjectSW*p_object_B,int subindex_B) { + + BroadPhaseOctree *bpo=(BroadPhaseOctree*)(self); + if (!bpo->pair_callback) + return NULL; + + return bpo->pair_callback(p_object_A,subindex_A,p_object_B,subindex_B,bpo->pair_userdata); + +} + +void BroadPhaseOctree::_unpair_callback(void*self,OctreeElementID p_A, CollisionObjectSW*p_object_A,int subindex_A,OctreeElementID p_B, CollisionObjectSW*p_object_B,int subindex_B,void*pairdata) { + + BroadPhaseOctree *bpo=(BroadPhaseOctree*)(self); + if (!bpo->unpair_callback) + return; + + bpo->unpair_callback(p_object_A,subindex_A,p_object_B,subindex_B,pairdata,bpo->unpair_userdata); + +} + + +void BroadPhaseOctree::set_pair_callback(PairCallback p_pair_callback,void *p_userdata){ + + pair_callback=p_pair_callback; + pair_userdata=p_userdata; + +} +void BroadPhaseOctree::set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata){ + + unpair_callback=p_unpair_callback; + unpair_userdata=p_userdata; + +} + +void BroadPhaseOctree::update() { + // does.. not? +} + +BroadPhaseSW *BroadPhaseOctree::_create() { + + return memnew( BroadPhaseOctree ); +} + +BroadPhaseOctree::BroadPhaseOctree() { + octree.set_pair_callback(_pair_callback,this); + octree.set_unpair_callback(_unpair_callback,this); + pair_callback=NULL; + pair_userdata=NULL; + pair_callback=NULL; + unpair_userdata=NULL; +} + + diff --git a/servers/physics/broad_phase_octree.h b/servers/physics/broad_phase_octree.h new file mode 100644 index 0000000000..d365213966 --- /dev/null +++ b/servers/physics/broad_phase_octree.h @@ -0,0 +1,73 @@ +/*************************************************************************/ +/* broad_phase_octree.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef BROAD_PHASE_OCTREE_H +#define BROAD_PHASE_OCTREE_H + +#include "broad_phase_sw.h" +#include "octree.h" + +class BroadPhaseOctree : public BroadPhaseSW { + + + Octree<CollisionObjectSW,true> octree; + + static void* _pair_callback(void*,OctreeElementID, CollisionObjectSW*,int,OctreeElementID, CollisionObjectSW*,int); + static void _unpair_callback(void*,OctreeElementID, CollisionObjectSW*,int,OctreeElementID, CollisionObjectSW*,int,void*); + + + PairCallback pair_callback; + void *pair_userdata; + UnpairCallback unpair_callback; + void *unpair_userdata; + +public: + + // 0 is an invalid ID + virtual ID create(CollisionObjectSW *p_object_, int p_subindex=0); + virtual void move(ID p_id, const AABB& p_aabb); + virtual void set_static(ID p_id, bool p_static); + virtual void remove(ID p_id); + + virtual CollisionObjectSW *get_object(ID p_id) const; + virtual bool is_static(ID p_id) const; + virtual int get_subindex(ID p_id) const; + + virtual int cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL); + virtual int cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL); + + virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata); + virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata); + + virtual void update(); + + static BroadPhaseSW *_create(); + BroadPhaseOctree(); +}; + +#endif // BROAD_PHASE_OCTREE_H diff --git a/servers/physics/broad_phase_sw.cpp b/servers/physics/broad_phase_sw.cpp new file mode 100644 index 0000000000..1211db141f --- /dev/null +++ b/servers/physics/broad_phase_sw.cpp @@ -0,0 +1,35 @@ +/*************************************************************************/ +/* broad_phase_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "broad_phase_sw.h" + +BroadPhaseSW::CreateFunction BroadPhaseSW::create_func=NULL; + +BroadPhaseSW::~BroadPhaseSW() +{ +} diff --git a/servers/physics/broad_phase_sw.h b/servers/physics/broad_phase_sw.h new file mode 100644 index 0000000000..57301a2af9 --- /dev/null +++ b/servers/physics/broad_phase_sw.h @@ -0,0 +1,73 @@ +/*************************************************************************/ +/* broad_phase_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef BROAD_PHASE_SW_H +#define BROAD_PHASE_SW_H + +#include "math_funcs.h" +#include "aabb.h" + +class CollisionObjectSW; + + +class BroadPhaseSW { + +public: + typedef BroadPhaseSW* (*CreateFunction)(); + + static CreateFunction create_func; + + typedef uint32_t ID; + + + typedef void* (*PairCallback)(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_userdata); + typedef void (*UnpairCallback)(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_data,void *p_userdata); + + // 0 is an invalid ID + virtual ID create(CollisionObjectSW *p_object_, int p_subindex=0)=0; + virtual void move(ID p_id, const AABB& p_aabb)=0; + virtual void set_static(ID p_id, bool p_static)=0; + virtual void remove(ID p_id)=0; + + virtual CollisionObjectSW *get_object(ID p_id) const=0; + virtual bool is_static(ID p_id) const=0; + virtual int get_subindex(ID p_id) const=0; + + virtual int cull_segment(const Vector3& p_from, const Vector3& p_to,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL)=0; + virtual int cull_aabb(const AABB& p_aabb,CollisionObjectSW** p_results,int p_max_results,int *p_result_indices=NULL)=0; + + virtual void set_pair_callback(PairCallback p_pair_callback,void *p_userdata)=0; + virtual void set_unpair_callback(UnpairCallback p_unpair_callback,void *p_userdata)=0; + + virtual void update()=0; + + virtual ~BroadPhaseSW(); + +}; + +#endif // BROAD_PHASE__SW_H diff --git a/servers/physics/collision_object_sw.cpp b/servers/physics/collision_object_sw.cpp new file mode 100644 index 0000000000..156004d15d --- /dev/null +++ b/servers/physics/collision_object_sw.cpp @@ -0,0 +1,219 @@ +/*************************************************************************/ +/* collision_object_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "collision_object_sw.h" +#include "space_sw.h" + +void CollisionObjectSW::add_shape(ShapeSW *p_shape,const Transform& p_transform) { + + Shape s; + s.shape=p_shape; + s.xform=p_transform; + s.xform_inv=s.xform.affine_inverse(); + s.bpid=0; //needs update + shapes.push_back(s); + p_shape->add_owner(this); + _update_shapes(); + _shapes_changed(); + +} + +void CollisionObjectSW::set_shape(int p_index,ShapeSW *p_shape){ + + ERR_FAIL_INDEX(p_index,shapes.size()); + shapes[p_index].shape->remove_owner(this); + shapes[p_index].shape=p_shape; + + p_shape->add_owner(this); + _update_shapes(); + _shapes_changed(); + +} +void CollisionObjectSW::set_shape_transform(int p_index,const Transform& p_transform){ + + ERR_FAIL_INDEX(p_index,shapes.size()); + + shapes[p_index].xform=p_transform; + shapes[p_index].xform_inv=p_transform.affine_inverse(); + _update_shapes(); + _shapes_changed(); +} + +void CollisionObjectSW::remove_shape(ShapeSW *p_shape) { + + //remove a shape, all the times it appears + for(int i=0;i<shapes.size();i++) { + + if (shapes[i].shape==p_shape) { + remove_shape(i); + i--; + } + } +} + +void CollisionObjectSW::remove_shape(int p_index){ + + //remove anything from shape to be erased to end, so subindices don't change + ERR_FAIL_INDEX(p_index,shapes.size()); + for(int i=p_index;i<shapes.size();i++) { + + if (shapes[i].bpid==0) + continue; + //should never get here with a null owner + space->get_broadphase()->remove(shapes[i].bpid); + shapes[i].bpid=0; + } + shapes[p_index].shape->remove_owner(this); + shapes.remove(p_index); + + _shapes_changed(); + +} + +void CollisionObjectSW::_set_static(bool p_static) { + if (_static==p_static) + return; + _static=p_static; + + if (!space) + return; + for(int i=0;i<get_shape_count();i++) { + Shape &s=shapes[i]; + if (s.bpid>0) { + space->get_broadphase()->set_static(s.bpid,_static); + } + } + +} + +void CollisionObjectSW::_unregister_shapes() { + + for(int i=0;i<shapes.size();i++) { + + Shape &s=shapes[i]; + if (s.bpid>0) { + space->get_broadphase()->remove(s.bpid); + s.bpid=0; + } + } + +} + +void CollisionObjectSW::_update_shapes() { + + if (!space) + return; + + for(int i=0;i<shapes.size();i++) { + + Shape &s=shapes[i]; + if (s.bpid==0) { + s.bpid=space->get_broadphase()->create(this,i); + space->get_broadphase()->set_static(s.bpid,_static); + } + + //not quite correct, should compute the next matrix.. + AABB shape_aabb=s.shape->get_aabb(); + Transform xform = transform * s.xform; + shape_aabb=xform.xform(shape_aabb); + s.aabb_cache=shape_aabb; + s.aabb_cache=s.aabb_cache.grow( (s.aabb_cache.size.x + s.aabb_cache.size.y)*0.5*0.05 ); + + + space->get_broadphase()->move(s.bpid,s.aabb_cache); + } + +} + +void CollisionObjectSW::_update_shapes_with_motion(const Vector3& p_motion) { + + + if (!space) + return; + + for(int i=0;i<shapes.size();i++) { + + Shape &s=shapes[i]; + if (s.bpid==0) { + s.bpid=space->get_broadphase()->create(this,i); + space->get_broadphase()->set_static(s.bpid,_static); + } + + //not quite correct, should compute the next matrix.. + AABB shape_aabb=s.shape->get_aabb(); + Transform xform = transform * s.xform; + shape_aabb=xform.xform(shape_aabb); + shape_aabb=shape_aabb.merge(AABB( shape_aabb.pos+p_motion,shape_aabb.size)); //use motion + s.aabb_cache=shape_aabb; + + space->get_broadphase()->move(s.bpid,shape_aabb); + } + + +} + +void CollisionObjectSW::_set_space(SpaceSW *p_space) { + + if (space) { + + space->remove_object(this); + + for(int i=0;i<shapes.size();i++) { + + Shape &s=shapes[i]; + if (s.bpid) { + space->get_broadphase()->remove(s.bpid); + s.bpid=0; + } + } + + } + + space=p_space; + + if (space) { + + space->add_object(this); + _update_shapes(); + } + +} + +void CollisionObjectSW::_shape_changed() { + + _update_shapes(); + _shapes_changed(); +} + +CollisionObjectSW::CollisionObjectSW(Type p_type) { + + _static=true; + type=p_type; + space=NULL; + instance_id=0; +} diff --git a/servers/physics/collision_object_sw.h b/servers/physics/collision_object_sw.h new file mode 100644 index 0000000000..6d60f2f078 --- /dev/null +++ b/servers/physics/collision_object_sw.h @@ -0,0 +1,119 @@ +/*************************************************************************/ +/* collision_object_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef COLLISION_OBJECT_SW_H +#define COLLISION_OBJECT_SW_H + +#include "shape_sw.h" +#include "servers/physics_server.h" +#include "self_list.h" +#include "broad_phase_sw.h" + +class SpaceSW; + +class CollisionObjectSW : public ShapeOwnerSW { +public: + enum Type { + TYPE_AREA, + TYPE_BODY + }; +private: + + Type type; + RID self; + ObjectID instance_id; + + struct Shape { + + Transform xform; + Transform xform_inv; + BroadPhaseSW::ID bpid; + AABB aabb_cache; //for rayqueries + ShapeSW *shape; + }; + + Vector<Shape> shapes; + SpaceSW *space; + Transform transform; + Transform inv_transform; + bool _static; + + void _update_shapes(); + +protected: + + + void _update_shapes_with_motion(const Vector3& p_motion); + void _unregister_shapes(); + + _FORCE_INLINE_ void _set_transform(const Transform& p_transform) { transform=p_transform; _update_shapes(); } + _FORCE_INLINE_ void _set_inv_transform(const Transform& p_transform) { inv_transform=p_transform; } + void _set_static(bool p_static); + + virtual void _shapes_changed()=0; + void _set_space(SpaceSW *space); + + CollisionObjectSW(Type p_type); +public: + + _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + _FORCE_INLINE_ void set_instance_id(const ObjectID& p_instance_id) { instance_id=p_instance_id; } + _FORCE_INLINE_ ObjectID get_instance_id() const { return instance_id; } + + void _shape_changed(); + + _FORCE_INLINE_ Type get_type() const { return type; } + void add_shape(ShapeSW *p_shape,const Transform& p_transform=Transform()); + void set_shape(int p_index,ShapeSW *p_shape); + void set_shape_transform(int p_index,const Transform& p_transform); + _FORCE_INLINE_ int get_shape_count() const { return shapes.size(); } + _FORCE_INLINE_ ShapeSW *get_shape(int p_index) const { return shapes[p_index].shape; } + _FORCE_INLINE_ const Transform& get_shape_transform(int p_index) const { return shapes[p_index].xform; } + _FORCE_INLINE_ const Transform& get_shape_inv_transform(int p_index) const { return shapes[p_index].xform_inv; } + _FORCE_INLINE_ const AABB& get_shape_aabb(int p_index) const { return shapes[p_index].aabb_cache; } + + _FORCE_INLINE_ Transform get_transform() const { return transform; } + _FORCE_INLINE_ Transform get_inv_transform() const { return inv_transform; } + _FORCE_INLINE_ SpaceSW* get_space() const { return space; } + + + + void remove_shape(ShapeSW *p_shape); + void remove_shape(int p_index); + + virtual void set_space(SpaceSW *p_space)=0; + + _FORCE_INLINE_ bool is_static() const { return _static; } + + virtual ~CollisionObjectSW() {} + +}; + +#endif // COLLISION_OBJECT_SW_H diff --git a/servers/physics/collision_solver_sat.cpp b/servers/physics/collision_solver_sat.cpp new file mode 100644 index 0000000000..1cd40db772 --- /dev/null +++ b/servers/physics/collision_solver_sat.cpp @@ -0,0 +1,1331 @@ +/*************************************************************************/ +/* collision_solver_sat.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "collision_solver_sat.h" +#include "geometry.h" + +#define _EDGE_IS_VALID_SUPPORT_TRESHOLD 0.02 + +struct _CollectorCallback { + + CollisionSolverSW::CallbackResult callback; + void *userdata; + bool swap; + bool collided; + Vector3 normal; + Vector3 *prev_axis; + + _FORCE_INLINE_ void call(const Vector3& p_point_A, const Vector3& p_point_B) { + + //if (normal.dot(p_point_A) >= normal.dot(p_point_B)) + // return; + if (swap) + callback(p_point_B,p_point_A,userdata); + else + callback(p_point_A,p_point_B,userdata); + } + +}; + +typedef void (*GenerateContactsFunc)(const Vector3 *,int, const Vector3 *,int ,_CollectorCallback *); + + +static void _generate_contacts_point_point(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) { + +#ifdef DEBUG_ENABLED + ERR_FAIL_COND( p_point_count_A != 1 ); + ERR_FAIL_COND( p_point_count_B != 1 ); +#endif + + p_callback->call(*p_points_A,*p_points_B); +} + +static void _generate_contacts_point_edge(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) { + +#ifdef DEBUG_ENABLED + ERR_FAIL_COND( p_point_count_A != 1 ); + ERR_FAIL_COND( p_point_count_B != 2 ); +#endif + + Vector3 closest_B = Geometry::get_closest_point_to_segment_uncapped(*p_points_A, p_points_B ); + p_callback->call(*p_points_A,closest_B); + +} + +static void _generate_contacts_point_face(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) { + +#ifdef DEBUG_ENABLED + ERR_FAIL_COND( p_point_count_A != 1 ); + ERR_FAIL_COND( p_point_count_B < 3 ); +#endif + + + Vector3 closest_B=Plane(p_points_B[0],p_points_B[1],p_points_B[2]).project( *p_points_A ); + + p_callback->call(*p_points_A,closest_B); + +} + + +static void _generate_contacts_edge_edge(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) { + +#ifdef DEBUG_ENABLED + ERR_FAIL_COND( p_point_count_A != 2 ); + ERR_FAIL_COND( p_point_count_B != 2 ); // circle is actually a 4x3 matrix +#endif + + + + Vector3 rel_A=p_points_A[1]-p_points_A[0]; + Vector3 rel_B=p_points_B[1]-p_points_B[0]; + + Vector3 c=rel_A.cross(rel_B).cross(rel_B); + +// if ( Math::abs(rel_A.dot(c) )<_EDGE_IS_VALID_SUPPORT_TRESHOLD ) { + if ( Math::abs(rel_A.dot(c) )<CMP_EPSILON ) { + + // should handle somehow.. + //ERR_PRINT("TODO FIX"); + //return; + + Vector3 axis = rel_A.normalized(); //make an axis + Vector3 base_A = p_points_A[0] - axis * axis.dot(p_points_A[0]); + Vector3 base_B = p_points_B[0] - axis * axis.dot(p_points_B[0]); + + //sort all 4 points in axis + float dvec[4]={ axis.dot(p_points_A[0]), axis.dot(p_points_A[1]), axis.dot(p_points_B[0]), axis.dot(p_points_B[1]) }; + + SortArray<float> sa; + sa.sort(dvec,4); + + //use the middle ones as contacts + p_callback->call(base_A+axis*dvec[1],base_B+axis*dvec[1]); + p_callback->call(base_A+axis*dvec[2],base_B+axis*dvec[2]); + + return; + + } + + real_t d = (c.dot( p_points_B[0] ) - p_points_A[0].dot(c))/rel_A.dot(c); + + if (d<0.0) + d=0.0; + else if (d>1.0) + d=1.0; + + Vector3 closest_A=p_points_A[0]+rel_A*d; + Vector3 closest_B=Geometry::get_closest_point_to_segment_uncapped(closest_A, p_points_B); + p_callback->call(closest_A,closest_B); + +} + +static void _generate_contacts_face_face(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) { + +#ifdef DEBUG_ENABLED + ERR_FAIL_COND( p_point_count_A <2 ); + ERR_FAIL_COND( p_point_count_B <3 ); +#endif + + static const int max_clip=32; + + Vector3 _clipbuf1[max_clip]; + Vector3 _clipbuf2[max_clip]; + Vector3 *clipbuf_src=_clipbuf1; + Vector3 *clipbuf_dst=_clipbuf2; + int clipbuf_len=p_point_count_A; + + // copy A points to clipbuf_src + for (int i=0;i<p_point_count_A;i++) { + + clipbuf_src[i]=p_points_A[i]; + } + + Plane plane_B(p_points_B[0],p_points_B[1],p_points_B[2]); + + // go through all of B points + for (int i=0;i<p_point_count_B;i++) { + + int i_n=(i+1)%p_point_count_B; + + Vector3 edge0_B=p_points_B[i]; + Vector3 edge1_B=p_points_B[i_n]; + + Vector3 clip_normal = (edge0_B - edge1_B).cross( plane_B.normal ).normalized(); + // make a clip plane + + + Plane clip(edge0_B,clip_normal); + // avoid double clip if A is edge + int dst_idx=0; + bool edge = clipbuf_len==2; + for (int j=0;j<clipbuf_len;j++) { + + int j_n=(j+1)%clipbuf_len; + + Vector3 edge0_A=clipbuf_src[j]; + Vector3 edge1_A=clipbuf_src[j_n]; + + real_t dist0 = clip.distance_to(edge0_A); + real_t dist1 = clip.distance_to(edge1_A); + + + if ( dist0 <= 0 ) { // behind plane + + ERR_FAIL_COND( dst_idx >= max_clip ); + clipbuf_dst[dst_idx++]=clipbuf_src[j]; + + } + + + // check for different sides and non coplanar +// if ( (dist0*dist1) < -CMP_EPSILON && !(edge && j)) { + if ( (dist0*dist1) < 0 && !(edge && j)) { + + // calculate intersection + Vector3 rel = edge1_A - edge0_A; + real_t den=clip.normal.dot( rel ); + real_t dist=-(clip.normal.dot( edge0_A )-clip.d)/den; + Vector3 inters = edge0_A+rel*dist; + + ERR_FAIL_COND( dst_idx >= max_clip ); + clipbuf_dst[dst_idx]=inters; + dst_idx++; + } + } + + clipbuf_len=dst_idx; + SWAP(clipbuf_src,clipbuf_dst); + } + + + // generate contacts + //Plane plane_A(p_points_A[0],p_points_A[1],p_points_A[2]); + + int added=0; + + for (int i=0;i<clipbuf_len;i++) { + + float d = plane_B.distance_to(clipbuf_src[i]); + if (d>CMP_EPSILON) + continue; + + Vector3 closest_B=clipbuf_src[i] - plane_B.normal*d; + + p_callback->call(clipbuf_src[i],closest_B); + added++; + + } + +} + + +static void _generate_contacts_from_supports(const Vector3 * p_points_A,int p_point_count_A, const Vector3 * p_points_B,int p_point_count_B,_CollectorCallback *p_callback) { + + +#ifdef DEBUG_ENABLED + ERR_FAIL_COND( p_point_count_A <1 ); + ERR_FAIL_COND( p_point_count_B <1 ); +#endif + + + static const GenerateContactsFunc generate_contacts_func_table[3][3]={ + { + _generate_contacts_point_point, + _generate_contacts_point_edge, + _generate_contacts_point_face, + },{ + 0, + _generate_contacts_edge_edge, + _generate_contacts_face_face, + },{ + 0,0, + _generate_contacts_face_face, + } + }; + + int pointcount_B; + int pointcount_A; + const Vector3 *points_A; + const Vector3 *points_B; + + if (p_point_count_A > p_point_count_B) { + //swap + p_callback->swap = !p_callback->swap; + p_callback->normal = -p_callback->normal; + + pointcount_B = p_point_count_A; + pointcount_A = p_point_count_B; + points_A=p_points_B; + points_B=p_points_A; + } else { + + pointcount_B = p_point_count_B; + pointcount_A = p_point_count_A; + points_A=p_points_A; + points_B=p_points_B; + } + + int version_A = (pointcount_A > 3 ? 3 : pointcount_A) -1; + int version_B = (pointcount_B > 3 ? 3 : pointcount_B) -1; + + GenerateContactsFunc contacts_func = generate_contacts_func_table[version_A][version_B]; + ERR_FAIL_COND(!contacts_func); + contacts_func(points_A,pointcount_A,points_B,pointcount_B,p_callback); + +} + + + +template<class ShapeA, class ShapeB> +class SeparatorAxisTest { + + const ShapeA *shape_A; + const ShapeB *shape_B; + const Transform *transform_A; + const Transform *transform_B; + real_t best_depth; + Vector3 best_axis; + _CollectorCallback *callback; + + Vector3 separator_axis; + +public: + + _FORCE_INLINE_ bool test_previous_axis() { + + if (callback && callback->prev_axis && *callback->prev_axis!=Vector3()) + return test_axis(*callback->prev_axis); + else + return true; + } + + _FORCE_INLINE_ bool test_axis(const Vector3& p_axis) { + + Vector3 axis=p_axis; + + if ( Math::abs(axis.x)<CMP_EPSILON && + Math::abs(axis.y)<CMP_EPSILON && + Math::abs(axis.z)<CMP_EPSILON ) { + // strange case, try an upwards separator + axis=Vector3(0.0,1.0,0.0); + } + + real_t min_A,max_A,min_B,max_B; + + shape_A->project_range(axis,*transform_A,min_A,max_A); + shape_B->project_range(axis,*transform_B,min_B,max_B); + + min_B -= ( max_A - min_A ) * 0.5; + max_B += ( max_A - min_A ) * 0.5; + + real_t dmin = min_B - ( min_A + max_A ) * 0.5; + real_t dmax = max_B - ( min_A + max_A ) * 0.5; + + if (dmin > 0.0 || dmax < 0.0) { + separator_axis=axis; + return false; // doesn't contain 0 + } + + //use the smallest depth + + dmin = Math::abs(dmin); + + if ( dmax < dmin ) { + if ( dmax < best_depth ) { + best_depth=dmax; + best_axis=axis; + } + } else { + if ( dmin < best_depth ) { + best_depth=dmin; + best_axis=-axis; // keep it as A axis + } + } + + return true; + } + + + _FORCE_INLINE_ void generate_contacts() { + + // nothing to do, don't generate + if (best_axis==Vector3(0.0,0.0,0.0)) + return; + + if (!callback->callback) { + //just was checking intersection? + callback->collided=true; + if (callback->prev_axis) + *callback->prev_axis=best_axis; + return; + } + + static const int max_supports=16; + + Vector3 supports_A[max_supports]; + int support_count_A; + shape_A->get_supports(transform_A->basis.xform_inv(-best_axis).normalized(),max_supports,supports_A,support_count_A); + for(int i=0;i<support_count_A;i++) { + supports_A[i] = transform_A->xform(supports_A[i]); + } + + + Vector3 supports_B[max_supports]; + int support_count_B; + shape_B->get_supports(transform_B->basis.xform_inv(best_axis).normalized(),max_supports,supports_B,support_count_B); + for(int i=0;i<support_count_B;i++) { + supports_B[i] = transform_B->xform(supports_B[i]); + } +/* + print_line("best depth: "+rtos(best_depth)); + for(int i=0;i<support_count_A;i++) { + + print_line("A-"+itos(i)+": "+supports_A[i]); + } + for(int i=0;i<support_count_B;i++) { + + print_line("B-"+itos(i)+": "+supports_B[i]); + } +*/ + callback->normal=best_axis; + if (callback->prev_axis) + *callback->prev_axis=best_axis; + _generate_contacts_from_supports(supports_A,support_count_A,supports_B,support_count_B,callback); + + callback->collided=true; + //CollisionSolverSW::CallbackResult cbk=NULL; + //cbk(Vector3(),Vector3(),NULL); + + } + + _FORCE_INLINE_ SeparatorAxisTest(const ShapeA *p_shape_A,const Transform& p_transform_A, const ShapeB *p_shape_B,const Transform& p_transform_B,_CollectorCallback *p_callback) { + best_depth=1e15; + shape_A=p_shape_A; + shape_B=p_shape_B; + transform_A=&p_transform_A; + transform_B=&p_transform_B; + callback=p_callback; + } + +}; + +/****** SAT TESTS *******/ +/****** SAT TESTS *******/ +/****** SAT TESTS *******/ +/****** SAT TESTS *******/ + + +typedef void (*CollisionFunc)(const ShapeSW*,const Transform&,const ShapeSW*,const Transform&,_CollectorCallback *p_callback); + + +static void _collision_sphere_sphere(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) { + + + const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a); + const SphereShapeSW *sphere_B = static_cast<const SphereShapeSW*>(p_b); + + SeparatorAxisTest<SphereShapeSW,SphereShapeSW> separator(sphere_A,p_transform_a,sphere_B,p_transform_b,p_collector); + + // previous axis + + if (!separator.test_previous_axis()) + return; + + if (!separator.test_axis( (p_transform_a.origin-p_transform_b.origin).normalized() )) + return; + + separator.generate_contacts(); +} + +static void _collision_sphere_box(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) { + + + const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a); + const BoxShapeSW *box_B = static_cast<const BoxShapeSW*>(p_b); + + SeparatorAxisTest<SphereShapeSW,BoxShapeSW> separator(sphere_A,p_transform_a,box_B,p_transform_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + // test faces + + for (int i=0;i<3;i++) { + + Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); + + if (!separator.test_axis( axis )) + return; + + } + + // calculate closest point to sphere + + Vector3 cnormal=p_transform_b.xform_inv( p_transform_a.origin ); + + Vector3 cpoint=p_transform_b.xform( Vector3( + + (cnormal.x<0) ? -box_B->get_half_extents().x : box_B->get_half_extents().x, + (cnormal.y<0) ? -box_B->get_half_extents().y : box_B->get_half_extents().y, + (cnormal.z<0) ? -box_B->get_half_extents().z : box_B->get_half_extents().z + ) ); + + // use point to test axis + Vector3 point_axis = (p_transform_a.origin - cpoint).normalized(); + + if (!separator.test_axis( point_axis )) + return; + + // test edges + + for (int i=0;i<3;i++) { + + Vector3 axis = point_axis.cross( p_transform_b.basis.get_axis(i) ).cross( p_transform_b.basis.get_axis(i) ).normalized(); + + if (!separator.test_axis( axis )) + return; + } + + separator.generate_contacts(); + + +} + + +static void _collision_sphere_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) { + + const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a); + const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW*>(p_b); + + SeparatorAxisTest<SphereShapeSW,CapsuleShapeSW> separator(sphere_A,p_transform_a,capsule_B,p_transform_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + //capsule sphere 1, sphere + + Vector3 capsule_axis = p_transform_b.basis.get_axis(2) * (capsule_B->get_height() * 0.5); + + Vector3 capsule_ball_1 = p_transform_b.origin + capsule_axis; + + if (!separator.test_axis( (capsule_ball_1 - p_transform_a.origin).normalized() ) ) + return; + + //capsule sphere 2, sphere + + Vector3 capsule_ball_2 = p_transform_b.origin - capsule_axis; + + if (!separator.test_axis( (capsule_ball_1 - p_transform_a.origin).normalized() ) ) + return; + + //capsule edge, sphere + + Vector3 b2a = p_transform_a.origin - p_transform_b.origin; + + Vector3 axis = b2a.cross( capsule_axis ).cross( capsule_axis ).normalized(); + + + if (!separator.test_axis( axis )) + return; + + separator.generate_contacts(); +} + +static void _collision_sphere_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) { + + + const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a); + const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b); + + SeparatorAxisTest<SphereShapeSW,ConvexPolygonShapeSW> separator(sphere_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector); + + + if (!separator.test_previous_axis()) + return; + + const Geometry::MeshData &mesh = convex_polygon_B->get_mesh(); + + const Geometry::MeshData::Face *faces = mesh.faces.ptr(); + int face_count = mesh.faces.size(); + const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); + int edge_count = mesh.edges.size(); + const Vector3 *vertices = mesh.vertices.ptr(); + int vertex_count = mesh.vertices.size(); + + + // faces of B + for (int i=0;i<face_count;i++) { + + Vector3 axis = p_transform_b.xform( faces[i].plane ).normal; + + if (!separator.test_axis( axis )) + return; + } + + + // edges of B + for(int i=0;i<edge_count;i++) { + + + Vector3 v1=p_transform_b.xform( vertices[ edges[i].a ] ); + Vector3 v2=p_transform_b.xform( vertices[ edges[i].b ] ); + Vector3 v3=p_transform_a.origin; + + + Vector3 n1=v2-v1; + Vector3 n2=v2-v3; + + Vector3 axis = n1.cross(n2).cross(n1).normalized();; + + if (!separator.test_axis( axis )) + return; + + } + + // vertices of B + for(int i=0;i<vertex_count;i++) { + + + Vector3 v1=p_transform_b.xform( vertices[i] ); + Vector3 v2=p_transform_a.origin; + + Vector3 axis = (v2-v1).normalized(); + + if (!separator.test_axis( axis )) + return; + + } + + separator.generate_contacts(); + + +} + +static void _collision_sphere_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b, _CollectorCallback *p_collector) { + + const SphereShapeSW *sphere_A = static_cast<const SphereShapeSW*>(p_a); + const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b); + + + + SeparatorAxisTest<SphereShapeSW,FaceShapeSW> separator(sphere_A,p_transform_a,face_B,p_transform_b,p_collector); + + + Vector3 vertex[3]={ + p_transform_b.xform( face_B->vertex[0] ), + p_transform_b.xform( face_B->vertex[1] ), + p_transform_b.xform( face_B->vertex[2] ), + }; + + if (!separator.test_axis( (vertex[0]-vertex[2]).cross(vertex[0]-vertex[1]).normalized() )) + return; + + // edges and points of B + for(int i=0;i<3;i++) { + + + Vector3 n1=vertex[i]-p_transform_a.origin; + + if (!separator.test_axis( n1.normalized() )) { + return; + } + + Vector3 n2=vertex[(i+1)%3]-vertex[i]; + + Vector3 axis = n1.cross(n2).cross(n2).normalized(); + + if (!separator.test_axis( axis )) { + return; + } + + } + + separator.generate_contacts(); +} + + + + + +static void _collision_box_box(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) { + + + const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a); + const BoxShapeSW *box_B = static_cast<const BoxShapeSW*>(p_b); + + SeparatorAxisTest<BoxShapeSW,BoxShapeSW> separator(box_A,p_transform_a,box_B,p_transform_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + // test faces of A + + for (int i=0;i<3;i++) { + + Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + + if (!separator.test_axis( axis )) + return; + + } + + // test faces of B + + for (int i=0;i<3;i++) { + + Vector3 axis = p_transform_b.basis.get_axis(i).normalized(); + + if (!separator.test_axis( axis )) + return; + + } + + // test combined edges + for (int i=0;i<3;i++) { + + for (int j=0;j<3;j++) { + + Vector3 axis = p_transform_a.basis.get_axis(i).cross( p_transform_b.basis.get_axis(j) ); + + if (axis.length_squared()<CMP_EPSILON) + continue; + axis.normalize(); + + + if (!separator.test_axis( axis )) { + return; + } + } + } + + separator.generate_contacts(); + + +} + + +static void _collision_box_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) { + + const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a); + const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW*>(p_b); + + SeparatorAxisTest<BoxShapeSW,CapsuleShapeSW> separator(box_A,p_transform_a,capsule_B,p_transform_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + // faces of A + for (int i=0;i<3;i++) { + + Vector3 axis = p_transform_a.basis.get_axis(i); + + if (!separator.test_axis( axis )) + return; + } + + + Vector3 cyl_axis = p_transform_b.basis.get_axis(2).normalized(); + + // edges of A, capsule cylinder + + for (int i=0;i<3;i++) { + + // cylinder + Vector3 box_axis = p_transform_a.basis.get_axis(i); + Vector3 axis = box_axis.cross( cyl_axis ); + if (axis.length_squared() < CMP_EPSILON) + continue; + + if (!separator.test_axis( axis.normalized() )) + return; + } + + // points of A, capsule cylinder + // this sure could be made faster somehow.. + + for (int i=0;i<2;i++) { + for (int j=0;j<2;j++) { + for (int k=0;k<2;k++) { + Vector3 he = box_A->get_half_extents(); + he.x*=(i*2-1); + he.y*=(j*2-1); + he.z*=(k*2-1); + Vector3 point=p_transform_a.origin; + for(int l=0;l<3;l++) + point+=p_transform_a.basis.get_axis(l)*he[l]; + + //Vector3 axis = (point - cyl_axis * cyl_axis.dot(point)).normalized(); + Vector3 axis = Plane(cyl_axis,0).project(point).normalized(); + + if (!separator.test_axis( axis )) + return; + } + } + } + + // capsule balls, edges of A + + for (int i=0;i<2;i++) { + + + Vector3 capsule_axis = p_transform_b.basis.get_axis(2)*(capsule_B->get_height()*0.5); + + Vector3 sphere_pos = p_transform_b.origin + ((i==0)?capsule_axis:-capsule_axis); + + + Vector3 cnormal=p_transform_a.xform_inv( sphere_pos ); + + Vector3 cpoint=p_transform_a.xform( Vector3( + + (cnormal.x<0) ? -box_A->get_half_extents().x : box_A->get_half_extents().x, + (cnormal.y<0) ? -box_A->get_half_extents().y : box_A->get_half_extents().y, + (cnormal.z<0) ? -box_A->get_half_extents().z : box_A->get_half_extents().z + ) ); + + // use point to test axis + Vector3 point_axis = (sphere_pos - cpoint).normalized(); + + if (!separator.test_axis( point_axis )) + return; + + // test edges of A + + for (int i=0;i<3;i++) { + + Vector3 axis = point_axis.cross( p_transform_a.basis.get_axis(i) ).cross( p_transform_a.basis.get_axis(i) ).normalized(); + + if (!separator.test_axis( axis )) + return; + } + } + + + separator.generate_contacts(); +} + + +static void _collision_box_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) { + + + + const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a); + const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b); + + SeparatorAxisTest<BoxShapeSW,ConvexPolygonShapeSW> separator(box_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + + const Geometry::MeshData &mesh = convex_polygon_B->get_mesh(); + + const Geometry::MeshData::Face *faces = mesh.faces.ptr(); + int face_count = mesh.faces.size(); + const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); + int edge_count = mesh.edges.size(); + const Vector3 *vertices = mesh.vertices.ptr(); + int vertex_count = mesh.vertices.size(); + + // faces of A + for (int i=0;i<3;i++) { + + Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + + if (!separator.test_axis( axis )) + return; + } + + // faces of B + for (int i=0;i<face_count;i++) { + + Vector3 axis = p_transform_b.xform( faces[i].plane ).normal; + + if (!separator.test_axis( axis )) + return; + } + + // A<->B edges + for (int i=0;i<3;i++) { + + Vector3 e1 = p_transform_a.basis.get_axis(i); + + for (int j=0;j<edge_count;j++) { + + Vector3 e2=p_transform_b.basis.xform(vertices[edges[j].a]) - p_transform_b.basis.xform(vertices[edges[j].b]); + + Vector3 axis=e1.cross( e2 ).normalized(); + + if (!separator.test_axis( axis )) + return; + + } + } + + separator.generate_contacts(); + + +} + +static void _collision_box_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b, _CollectorCallback *p_collector) { + + + const BoxShapeSW *box_A = static_cast<const BoxShapeSW*>(p_a); + const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b); + + SeparatorAxisTest<BoxShapeSW,FaceShapeSW> separator(box_A,p_transform_a,face_B,p_transform_b,p_collector); + + Vector3 vertex[3]={ + p_transform_b.xform( face_B->vertex[0] ), + p_transform_b.xform( face_B->vertex[1] ), + p_transform_b.xform( face_B->vertex[2] ), + }; + + if (!separator.test_axis( (vertex[0]-vertex[2]).cross(vertex[0]-vertex[1]).normalized() )) + return; + + // faces of A + for (int i=0;i<3;i++) { + + Vector3 axis = p_transform_a.basis.get_axis(i).normalized(); + + if (!separator.test_axis( axis )) + return; + } + + // combined edges + for(int i=0;i<3;i++) { + + Vector3 e=vertex[i]-vertex[(i+1)%3]; + + for (int i=0;i<3;i++) { + + Vector3 axis = p_transform_a.basis.get_axis(i); + + if (!separator.test_axis( e.cross(axis).normalized() )) + return; + } + + } + + separator.generate_contacts(); + +} + +static void _collision_capsule_capsule(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) { + + const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW*>(p_a); + const CapsuleShapeSW *capsule_B = static_cast<const CapsuleShapeSW*>(p_b); + + SeparatorAxisTest<CapsuleShapeSW,CapsuleShapeSW> separator(capsule_A,p_transform_a,capsule_B,p_transform_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + // some values + + Vector3 capsule_A_axis = p_transform_a.basis.get_axis(2) * (capsule_A->get_height() * 0.5); + Vector3 capsule_B_axis = p_transform_b.basis.get_axis(2) * (capsule_B->get_height() * 0.5); + + Vector3 capsule_A_ball_1 = p_transform_a.origin + capsule_A_axis; + Vector3 capsule_A_ball_2 = p_transform_a.origin - capsule_A_axis; + Vector3 capsule_B_ball_1 = p_transform_b.origin + capsule_B_axis; + Vector3 capsule_B_ball_2 = p_transform_b.origin - capsule_B_axis; + + //balls-balls + + if (!separator.test_axis( (capsule_A_ball_1 - capsule_B_ball_1 ).normalized() ) ) + return; + if (!separator.test_axis( (capsule_A_ball_1 - capsule_B_ball_2 ).normalized() ) ) + return; + + if (!separator.test_axis( (capsule_A_ball_2 - capsule_B_ball_1 ).normalized() ) ) + return; + if (!separator.test_axis( (capsule_A_ball_2 - capsule_B_ball_2 ).normalized() ) ) + return; + + + // edges-balls + + if (!separator.test_axis( (capsule_A_ball_1 - capsule_B_ball_1 ).cross(capsule_A_axis).cross(capsule_A_axis).normalized() ) ) + return; + + if (!separator.test_axis( (capsule_A_ball_1 - capsule_B_ball_2 ).cross(capsule_A_axis).cross(capsule_A_axis).normalized() ) ) + return; + + if (!separator.test_axis( (capsule_B_ball_1 - capsule_A_ball_1 ).cross(capsule_B_axis).cross(capsule_B_axis).normalized() ) ) + return; + + if (!separator.test_axis( (capsule_B_ball_1 - capsule_A_ball_2 ).cross(capsule_B_axis).cross(capsule_B_axis).normalized() ) ) + return; + + // edges + + if (!separator.test_axis( capsule_A_axis.cross(capsule_B_axis).normalized() ) ) + return; + + + separator.generate_contacts(); + +} + +static void _collision_capsule_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) { + + + const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW*>(p_a); + const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b); + + SeparatorAxisTest<CapsuleShapeSW,ConvexPolygonShapeSW> separator(capsule_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + const Geometry::MeshData &mesh = convex_polygon_B->get_mesh(); + + const Geometry::MeshData::Face *faces = mesh.faces.ptr(); + int face_count = mesh.faces.size(); + const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); + int edge_count = mesh.edges.size(); + const Vector3 *vertices = mesh.vertices.ptr(); + int vertex_count = mesh.vertices.size(); + + // faces of B + for (int i=0;i<face_count;i++) { + + Vector3 axis = p_transform_b.xform( faces[i].plane ).normal; + + if (!separator.test_axis( axis )) + return; + } + + // edges of B, capsule cylinder + + for (int i=0;i<edge_count;i++) { + + // cylinder + Vector3 edge_axis = p_transform_b.basis.xform( vertices[ edges[i].a] ) - p_transform_b.basis.xform( vertices[ edges[i].b] ); + Vector3 axis = edge_axis.cross( p_transform_a.basis.get_axis(2) ).normalized(); + + + if (!separator.test_axis( axis )) + return; + } + + // capsule balls, edges of B + + for (int i=0;i<2;i++) { + + // edges of B, capsule cylinder + + Vector3 capsule_axis = p_transform_a.basis.get_axis(2)*(capsule_A->get_height()*0.5); + + Vector3 sphere_pos = p_transform_a.origin + ((i==0)?capsule_axis:-capsule_axis); + + for (int j=0;j<edge_count;j++) { + + + Vector3 n1=sphere_pos - p_transform_b.xform( vertices[ edges[j].a] ); + Vector3 n2=p_transform_b.basis.xform( vertices[ edges[j].a] ) - p_transform_b.basis.xform( vertices[ edges[j].b] ); + + Vector3 axis = n1.cross(n2).cross(n2).normalized(); + + if (!separator.test_axis( axis )) + return; + } + } + + + separator.generate_contacts(); + +} + +static void _collision_capsule_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b, _CollectorCallback *p_collector) { + + const CapsuleShapeSW *capsule_A = static_cast<const CapsuleShapeSW*>(p_a); + const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b); + + SeparatorAxisTest<CapsuleShapeSW,FaceShapeSW> separator(capsule_A,p_transform_a,face_B,p_transform_b,p_collector); + + + + Vector3 vertex[3]={ + p_transform_b.xform( face_B->vertex[0] ), + p_transform_b.xform( face_B->vertex[1] ), + p_transform_b.xform( face_B->vertex[2] ), + }; + + if (!separator.test_axis( (vertex[0]-vertex[2]).cross(vertex[0]-vertex[1]).normalized() )) + return; + + // edges of B, capsule cylinder + + Vector3 capsule_axis = p_transform_a.basis.get_axis(2)*(capsule_A->get_height()*0.5); + + for (int i=0;i<3;i++) { + + // edge-cylinder + Vector3 edge_axis = vertex[i]-vertex[(i+1)%3]; + Vector3 axis = edge_axis.cross( capsule_axis ).normalized(); + + if (!separator.test_axis( axis )) + return; + + if (!separator.test_axis( (p_transform_a.origin-vertex[i]).cross(capsule_axis).cross(capsule_axis).normalized() )) + return; + + for (int j=0;j<2;j++) { + + // point-spheres + Vector3 sphere_pos = p_transform_a.origin + ( (j==0) ? capsule_axis : -capsule_axis ); + + Vector3 n1=sphere_pos - vertex[i]; + + if (!separator.test_axis( n1.normalized() )) + return; + + Vector3 n2=edge_axis; + + axis = n1.cross(n2).cross(n2); + + if (!separator.test_axis( axis.normalized() )) + return; + + + } + + } + + + separator.generate_contacts(); + +} + + +static void _collision_convex_polygon_convex_polygon(const ShapeSW *p_a,const Transform &p_transform_a,const ShapeSW *p_b,const Transform &p_transform_b,_CollectorCallback *p_collector) { + + + const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW*>(p_a); + const ConvexPolygonShapeSW *convex_polygon_B = static_cast<const ConvexPolygonShapeSW*>(p_b); + + SeparatorAxisTest<ConvexPolygonShapeSW,ConvexPolygonShapeSW> separator(convex_polygon_A,p_transform_a,convex_polygon_B,p_transform_b,p_collector); + + if (!separator.test_previous_axis()) + return; + + const Geometry::MeshData &mesh_A = convex_polygon_A->get_mesh(); + + const Geometry::MeshData::Face *faces_A = mesh_A.faces.ptr(); + int face_count_A = mesh_A.faces.size(); + const Geometry::MeshData::Edge *edges_A = mesh_A.edges.ptr(); + int edge_count_A = mesh_A.edges.size(); + const Vector3 *vertices_A = mesh_A.vertices.ptr(); + int vertex_count_A = mesh_A.vertices.size(); + + const Geometry::MeshData &mesh_B = convex_polygon_B->get_mesh(); + + const Geometry::MeshData::Face *faces_B = mesh_B.faces.ptr(); + int face_count_B = mesh_B.faces.size(); + const Geometry::MeshData::Edge *edges_B = mesh_B.edges.ptr(); + int edge_count_B = mesh_B.edges.size(); + const Vector3 *vertices_B = mesh_B.vertices.ptr(); + int vertex_count_B = mesh_B.vertices.size(); + + // faces of A + for (int i=0;i<face_count_A;i++) { + + Vector3 axis = p_transform_a.xform( faces_A[i].plane ).normal; +// Vector3 axis = p_transform_a.basis.xform( faces_A[i].plane.normal ).normalized(); + + if (!separator.test_axis( axis )) + return; + } + + // faces of B + for (int i=0;i<face_count_B;i++) { + + Vector3 axis = p_transform_b.xform( faces_B[i].plane ).normal; +// Vector3 axis = p_transform_b.basis.xform( faces_B[i].plane.normal ).normalized(); + + + if (!separator.test_axis( axis )) + return; + } + + // A<->B edges + for (int i=0;i<edge_count_A;i++) { + + Vector3 e1=p_transform_a.basis.xform( vertices_A[ edges_A[i].a] ) -p_transform_a.basis.xform( vertices_A[ edges_A[i].b] ); + + for (int j=0;j<edge_count_B;j++) { + + Vector3 e2=p_transform_b.basis.xform( vertices_B[ edges_B[j].a] ) -p_transform_b.basis.xform( vertices_B[ edges_B[j].b] ); + + Vector3 axis=e1.cross( e2 ).normalized(); + + if (!separator.test_axis( axis )) + return; + + } + } + + separator.generate_contacts(); + +} + +static void _collision_convex_polygon_face(const ShapeSW *p_a,const Transform &p_transform_a, const ShapeSW *p_b,const Transform& p_transform_b, _CollectorCallback *p_collector) { + + + const ConvexPolygonShapeSW *convex_polygon_A = static_cast<const ConvexPolygonShapeSW*>(p_a); + const FaceShapeSW *face_B = static_cast<const FaceShapeSW*>(p_b); + + SeparatorAxisTest<ConvexPolygonShapeSW,FaceShapeSW> separator(convex_polygon_A,p_transform_a,face_B,p_transform_b,p_collector); + + const Geometry::MeshData &mesh = convex_polygon_A->get_mesh(); + + const Geometry::MeshData::Face *faces = mesh.faces.ptr(); + int face_count = mesh.faces.size(); + const Geometry::MeshData::Edge *edges = mesh.edges.ptr(); + int edge_count = mesh.edges.size(); + const Vector3 *vertices = mesh.vertices.ptr(); + int vertex_count = mesh.vertices.size(); + + + + Vector3 vertex[3]={ + p_transform_b.xform( face_B->vertex[0] ), + p_transform_b.xform( face_B->vertex[1] ), + p_transform_b.xform( face_B->vertex[2] ), + }; + + if (!separator.test_axis( (vertex[0]-vertex[2]).cross(vertex[0]-vertex[1]).normalized() )) + return; + + + // faces of A + for (int i=0;i<face_count;i++) { + +// Vector3 axis = p_transform_a.xform( faces[i].plane ).normal; + Vector3 axis = p_transform_a.basis.xform( faces[i].plane.normal ).normalized(); + + if (!separator.test_axis( axis )) + return; + } + + + // A<->B edges + for (int i=0;i<edge_count;i++) { + + Vector3 e1=p_transform_a.xform( vertices[edges[i].a] ) - p_transform_a.xform( vertices[edges[i].b] ); + + for (int j=0;j<3;j++) { + + Vector3 e2=vertex[j]-vertex[(j+1)%3]; + + Vector3 axis=e1.cross( e2 ).normalized(); + + if (!separator.test_axis( axis )) + return; + } + } + + separator.generate_contacts(); + +} + + +bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata,bool p_swap,Vector3* r_prev_axis) { + + PhysicsServer::ShapeType type_A=p_shape_A->get_type(); + + ERR_FAIL_COND_V(type_A==PhysicsServer::SHAPE_PLANE,false); + ERR_FAIL_COND_V(type_A==PhysicsServer::SHAPE_RAY,false); + ERR_FAIL_COND_V(p_shape_A->is_concave(),false); + + PhysicsServer::ShapeType type_B=p_shape_B->get_type(); + + ERR_FAIL_COND_V(type_B==PhysicsServer::SHAPE_PLANE,false); + ERR_FAIL_COND_V(type_B==PhysicsServer::SHAPE_RAY,false); + ERR_FAIL_COND_V(p_shape_B->is_concave(),false); + + + static const CollisionFunc collision_table[5][5]={ + {_collision_sphere_sphere, + _collision_sphere_box, + _collision_sphere_capsule, + _collision_sphere_convex_polygon, + _collision_sphere_face}, + {0, + _collision_box_box, + _collision_box_capsule, + _collision_box_convex_polygon, + _collision_box_face}, + {0, + 0, + _collision_capsule_capsule, + _collision_capsule_convex_polygon, + _collision_capsule_face}, + {0, + 0, + 0, + _collision_convex_polygon_convex_polygon, + _collision_convex_polygon_face}, + {0, + 0, + 0, + 0, + 0}, + }; + + _CollectorCallback callback; + callback.callback=p_result_callback; + callback.swap=p_swap; + callback.userdata=p_userdata; + callback.collided=false; + callback.prev_axis=r_prev_axis; + + const ShapeSW *A=p_shape_A; + const ShapeSW *B=p_shape_B; + const Transform *transform_A=&p_transform_A; + const Transform *transform_B=&p_transform_B; + + if (type_A > type_B) { + SWAP(A,B); + SWAP(transform_A,transform_B); + SWAP(type_A,type_B); + callback.swap = !callback.swap; + } + + + CollisionFunc collision_func = collision_table[type_A-2][type_B-2]; + ERR_FAIL_COND_V(!collision_func,false); + + + collision_func(A,*transform_A,B,*transform_B,&callback); + + return callback.collided; + +} diff --git a/servers/physics/collision_solver_sat.h b/servers/physics/collision_solver_sat.h new file mode 100644 index 0000000000..5023a17810 --- /dev/null +++ b/servers/physics/collision_solver_sat.h @@ -0,0 +1,37 @@ +/*************************************************************************/ +/* collision_solver_sat.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef COLLISION_SOLVER_SAT_H +#define COLLISION_SOLVER_SAT_H + +#include "collision_solver_sw.h" + + +bool sat_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false,Vector3* r_prev_axis=NULL); + +#endif // COLLISION_SOLVER_SAT_H diff --git a/servers/physics/collision_solver_sw.cpp b/servers/physics/collision_solver_sw.cpp new file mode 100644 index 0000000000..da28a4934f --- /dev/null +++ b/servers/physics/collision_solver_sw.cpp @@ -0,0 +1,241 @@ +/*************************************************************************/ +/* collision_solver_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "collision_solver_sw.h" +#include "collision_solver_sat.h" + +#include "gjk_epa.h" +#include "collision_solver_sat.h" + + +#define collision_solver sat_calculate_penetration +//#define collision_solver gjk_epa_calculate_penetration + + +bool CollisionSolverSW::solve_static_plane(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) { + + const PlaneShapeSW *plane = static_cast<const PlaneShapeSW*>(p_shape_A); + if (p_shape_B->get_type()==PhysicsServer::SHAPE_PLANE) + return false; + Plane p = p_transform_A.xform(plane->get_plane()); + + static const int max_supports = 16; + Vector3 supports[max_supports]; + int support_count; + + p_shape_B->get_supports(p_transform_B.basis.xform_inv(-p.normal).normalized(),max_supports,supports,support_count); + + bool found=false; + + for(int i=0;i<support_count;i++) { + + supports[i] = p_transform_B.xform( supports[i] ); + if (p.distance_to(supports[i])>=0) + continue; + found=true; + + Vector3 support_A = p.project(supports[i]); + + if (p_result_callback) { + if (p_swap_result) + p_result_callback(supports[i],support_A,p_userdata); + else + p_result_callback(support_A,supports[i],p_userdata); + } + + } + + + return found; +} + +bool CollisionSolverSW::solve_ray(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) { + + + const RayShapeSW *ray = static_cast<const RayShapeSW*>(p_shape_A); + + Vector3 from = p_transform_A.origin; + Vector3 to = from+p_transform_A.basis.get_axis(2)*ray->get_length(); + Vector3 support_A=to; + + Transform ai = p_transform_B.affine_inverse(); + + from = ai.xform(from); + to = ai.xform(to); + + Vector3 p,n; + if (!p_shape_B->intersect_segment(from,to,p,n)) + return false; + + Vector3 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; +} + +struct _ConcaveCollisionInfo { + + const Transform *transform_A; + const ShapeSW *shape_A; + const Transform *transform_B; + CollisionSolverSW::CallbackResult result_callback; + void *userdata; + bool swap_result; + bool collided; + int aabb_tests; + int collisions; + +}; + +void CollisionSolverSW::concave_callback(void *p_userdata, ShapeSW *p_convex) { + + + _ConcaveCollisionInfo &cinfo = *(_ConcaveCollisionInfo*)(p_userdata); + cinfo.aabb_tests++; + + bool collided = collision_solver(cinfo.shape_A, *cinfo.transform_A, p_convex,*cinfo.transform_B, cinfo.result_callback, cinfo.userdata, cinfo.swap_result ); + if (!collided) + return; + + cinfo.collided=true; + cinfo.collisions++; + +} + +bool CollisionSolverSW::solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result) { + + + const ConcaveShapeSW *concave_B=static_cast<const ConcaveShapeSW*>(p_shape_B); + + _ConcaveCollisionInfo cinfo; + cinfo.transform_A=&p_transform_A; + cinfo.shape_A=p_shape_A; + cinfo.transform_B=&p_transform_B; + cinfo.result_callback=p_result_callback; + cinfo.userdata=p_userdata; + cinfo.swap_result=p_swap_result; + cinfo.collided=false; + cinfo.collisions=0; + + cinfo.aabb_tests=0; + + Transform rel_transform = p_transform_A; + rel_transform.origin-=p_transform_B.origin; + + //quickly compute a local AABB + + AABB local_aabb; + for(int i=0;i<3;i++) { + + Vector3 axis( p_transform_B.basis.get_axis(i) ); + float axis_scale = 1.0/axis.length(); + axis*=axis_scale; + + float smin,smax; + p_shape_A->project_range(axis,rel_transform,smin,smax); + smin*=axis_scale; + smax*=axis_scale; + + local_aabb.pos[i]=smin; + local_aabb.size[i]=smax-smin; + } + + concave_B->cull(local_aabb,concave_callback,&cinfo); + + + return cinfo.collided; +} + + +bool CollisionSolverSW::solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis) { + + + PhysicsServer::ShapeType type_A=p_shape_A->get_type(); + PhysicsServer::ShapeType type_B=p_shape_B->get_type(); + bool concave_A=p_shape_A->is_concave(); + bool concave_B=p_shape_B->is_concave(); + + bool swap = false; + + if (type_A>type_B) { + SWAP(type_A,type_B); + SWAP(concave_A,concave_B); + swap=true; + } + + if (type_A==PhysicsServer::SHAPE_PLANE) { + + if (type_B==PhysicsServer::SHAPE_PLANE) + return false; + if (type_B==PhysicsServer::SHAPE_RAY) { + return false; + } + + if (swap) { + return solve_static_plane(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true); + } else { + return solve_static_plane(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false); + } + + } else if (type_A==PhysicsServer::SHAPE_RAY) { + + if (type_B==PhysicsServer::SHAPE_RAY) + return false; + + if (swap) { + return solve_ray(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true); + } else { + return solve_ray(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false); + } + + } else if (concave_B) { + + + if (concave_A) + return false; + + if (!swap) + return solve_concave(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_result_callback,p_userdata,false); + else + return solve_concave(p_shape_B,p_transform_B,p_shape_A,p_transform_A,p_result_callback,p_userdata,true); + + + + } else { + + return collision_solver(p_shape_A, p_transform_A, p_shape_B, p_transform_B, p_result_callback,p_userdata,false,r_sep_axis); + } + + + return false; +} diff --git a/servers/physics/collision_solver_sw.h b/servers/physics/collision_solver_sw.h new file mode 100644 index 0000000000..e135ab92e0 --- /dev/null +++ b/servers/physics/collision_solver_sw.h @@ -0,0 +1,52 @@ +/*************************************************************************/ +/* collision_solver_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef COLLISION_SOLVER_SW_H +#define COLLISION_SOLVER_SW_H + +#include "shape_sw.h" + +class CollisionSolverSW +{ +public: + typedef void (*CallbackResult)(const Vector3& p_point_A,const Vector3& p_point_B,void *p_userdata); +private: + + static void concave_callback(void *p_userdata, ShapeSW *p_convex); + static bool solve_static_plane(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result); + static bool solve_ray(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result); + static bool solve_concave(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,bool p_swap_result); + +public: + + + static bool solve_static(const ShapeSW *p_shape_A,const Transform& p_transform_A,const ShapeSW *p_shape_B,const Transform& p_transform_B,CallbackResult p_result_callback,void *p_userdata,Vector3 *r_sep_axis=NULL); + +}; + +#endif // COLLISION_SOLVER__SW_H diff --git a/servers/physics/constraint_sw.cpp b/servers/physics/constraint_sw.cpp new file mode 100644 index 0000000000..f1179bdb5c --- /dev/null +++ b/servers/physics/constraint_sw.cpp @@ -0,0 +1,30 @@ +/*************************************************************************/ +/* constraint_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "constraint_sw.h" + diff --git a/servers/physics/constraint_sw.h b/servers/physics/constraint_sw.h new file mode 100644 index 0000000000..1be96422e1 --- /dev/null +++ b/servers/physics/constraint_sw.h @@ -0,0 +1,72 @@ +/*************************************************************************/ +/* constraint_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef CONSTRAINT_SW_H +#define CONSTRAINT_SW_H + +#include "body_sw.h" + +class ConstraintSW { + + BodySW **_body_ptr; + int _body_count; + uint64_t island_step; + ConstraintSW *island_next; + ConstraintSW *island_list_next; + + + RID self; + +protected: + ConstraintSW(BodySW **p_body_ptr=NULL,int p_body_count=0) { _body_ptr=p_body_ptr; _body_count=p_body_count; island_step=0; } +public: + + _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; } + _FORCE_INLINE_ RID get_self() const { return self; } + + _FORCE_INLINE_ uint64_t get_island_step() const { return island_step; } + _FORCE_INLINE_ void set_island_step(uint64_t p_step) { island_step=p_step; } + + + _FORCE_INLINE_ ConstraintSW* get_island_next() const { return island_next; } + _FORCE_INLINE_ void set_island_next(ConstraintSW* p_next) { island_next=p_next; } + + _FORCE_INLINE_ ConstraintSW* get_island_list_next() const { return island_list_next; } + _FORCE_INLINE_ void set_island_list_next(ConstraintSW* p_next) { island_list_next=p_next; } + + _FORCE_INLINE_ BodySW **get_body_ptr() const { return _body_ptr; } + _FORCE_INLINE_ int get_body_count() const { return _body_count; } + + + virtual bool setup(float p_step)=0; + virtual void solve(float p_step)=0; + + virtual ~ConstraintSW() {} +}; + +#endif // CONSTRAINT__SW_H diff --git a/servers/physics/gjk_epa.cpp b/servers/physics/gjk_epa.cpp new file mode 100644 index 0000000000..37edc0d414 --- /dev/null +++ b/servers/physics/gjk_epa.cpp @@ -0,0 +1,900 @@ +/*************************************************************************/ +/* gjk_epa.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "gjk_epa.h" + +/*************** Bullet's GJK-EPA2 IMPLEMENTATION *******************/ + + // Config + +/* GJK */ +#define GJK_MAX_ITERATIONS 128 +#define GJK_ACCURARY ((real_t)0.0001) +#define GJK_MIN_DISTANCE ((real_t)0.0001) +#define GJK_DUPLICATED_EPS ((real_t)0.0001) +#define GJK_SIMPLEX2_EPS ((real_t)0.0) +#define GJK_SIMPLEX3_EPS ((real_t)0.0) +#define GJK_SIMPLEX4_EPS ((real_t)0.0) + +/* EPA */ +#define EPA_MAX_VERTICES 64 +#define EPA_MAX_FACES (EPA_MAX_VERTICES*2) +#define EPA_MAX_ITERATIONS 255 +#define EPA_ACCURACY ((real_t)0.0001) +#define EPA_FALLBACK (10*EPA_ACCURACY) +#define EPA_PLANE_EPS ((real_t)0.00001) +#define EPA_INSIDE_EPS ((real_t)0.01) + +namespace GjkEpa2 { + + +struct sResults { + enum eStatus { + Separated, /* Shapes doesnt penetrate */ + Penetrating, /* Shapes are penetrating */ + GJK_Failed, /* GJK phase fail, no big issue, shapes are probably just 'touching' */ + EPA_Failed /* EPA phase fail, bigger problem, need to save parameters, and debug */ + } status; + + Vector3 witnesses[2]; + Vector3 normal; + real_t distance; +}; + +// Shorthands +typedef unsigned int U; +typedef unsigned char U1; + +// MinkowskiDiff +struct MinkowskiDiff { + + const ShapeSW* m_shapes[2]; + + Transform transform_A; + Transform transform_B; + + // i wonder how this could be sped up... if it can + _FORCE_INLINE_ Vector3 Support0 ( const Vector3& d ) const { + return transform_A.xform( m_shapes[0]->get_support( transform_A.basis.xform_inv(d).normalized() ) ); + } + + _FORCE_INLINE_ Vector3 Support1 ( const Vector3& d ) const { + return transform_B.xform( m_shapes[1]->get_support( transform_B.basis.xform_inv(d).normalized() ) ); + } + + _FORCE_INLINE_ Vector3 Support ( const Vector3& d ) const { + return ( Support0 ( d )-Support1 ( -d ) ); + } + + _FORCE_INLINE_ Vector3 Support ( const Vector3& d,U index ) const + { + if ( index ) + return ( Support1 ( d ) ); + else + return ( Support0 ( d ) ); + } +}; + +typedef MinkowskiDiff tShape; + + +// GJK +struct GJK +{ + /* Types */ + struct sSV + { + Vector3 d,w; + }; + struct sSimplex + { + sSV* c[4]; + real_t p[4]; + U rank; + }; + struct eStatus { enum _ { + Valid, + Inside, + Failed };}; + /* Fields */ + tShape m_shape; + Vector3 m_ray; + real_t m_distance; + sSimplex m_simplices[2]; + sSV m_store[4]; + sSV* m_free[4]; + U m_nfree; + U m_current; + sSimplex* m_simplex; + eStatus::_ m_status; + /* Methods */ + GJK() + { + Initialize(); + } + void Initialize() + { + m_ray = Vector3(0,0,0); + m_nfree = 0; + m_status = eStatus::Failed; + m_current = 0; + m_distance = 0; + } + eStatus::_ Evaluate(const tShape& shapearg,const Vector3& guess) + { + U iterations=0; + real_t sqdist=0; + real_t alpha=0; + Vector3 lastw[4]; + U clastw=0; + /* Initialize solver */ + m_free[0] = &m_store[0]; + m_free[1] = &m_store[1]; + m_free[2] = &m_store[2]; + m_free[3] = &m_store[3]; + m_nfree = 4; + m_current = 0; + m_status = eStatus::Valid; + m_shape = shapearg; + m_distance = 0; + /* Initialize simplex */ + m_simplices[0].rank = 0; + m_ray = guess; + const real_t sqrl= m_ray.length_squared(); + appendvertice(m_simplices[0],sqrl>0?-m_ray:Vector3(1,0,0)); + m_simplices[0].p[0] = 1; + m_ray = m_simplices[0].c[0]->w; + sqdist = sqrl; + lastw[0] = + lastw[1] = + lastw[2] = + lastw[3] = m_ray; + /* Loop */ + do { + const U next=1-m_current; + sSimplex& cs=m_simplices[m_current]; + sSimplex& ns=m_simplices[next]; + /* Check zero */ + const real_t rl=m_ray.length(); + if(rl<GJK_MIN_DISTANCE) + {/* Touching or inside */ + m_status=eStatus::Inside; + break; + } + /* Append new vertice in -'v' direction */ + appendvertice(cs,-m_ray); + const Vector3& w=cs.c[cs.rank-1]->w; + bool found=false; + for(U i=0;i<4;++i) + { + if((w-lastw[i]).length_squared()<GJK_DUPLICATED_EPS) + { found=true;break; } + } + if(found) + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + else + {/* Update lastw */ + lastw[clastw=(clastw+1)&3]=w; + } + /* Check for termination */ + const real_t omega=vec3_dot(m_ray,w)/rl; + alpha=MAX(omega,alpha); + if(((rl-alpha)-(GJK_ACCURARY*rl))<=0) + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + /* Reduce simplex */ + real_t weights[4]; + U mask=0; + switch(cs.rank) + { + case 2: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + weights,mask);break; + case 3: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + weights,mask);break; + case 4: sqdist=projectorigin( cs.c[0]->w, + cs.c[1]->w, + cs.c[2]->w, + cs.c[3]->w, + weights,mask);break; + } + if(sqdist>=0) + {/* Valid */ + ns.rank = 0; + m_ray = Vector3(0,0,0); + m_current = next; + for(U i=0,ni=cs.rank;i<ni;++i) + { + if(mask&(1<<i)) + { + ns.c[ns.rank] = cs.c[i]; + ns.p[ns.rank++] = weights[i]; + m_ray += cs.c[i]->w*weights[i]; + } + else + { + m_free[m_nfree++] = cs.c[i]; + } + } + if(mask==15) m_status=eStatus::Inside; + } + else + {/* Return old simplex */ + removevertice(m_simplices[m_current]); + break; + } + m_status=((++iterations)<GJK_MAX_ITERATIONS)?m_status:eStatus::Failed; + } while(m_status==eStatus::Valid); + m_simplex=&m_simplices[m_current]; + switch(m_status) + { + case eStatus::Valid: m_distance=m_ray.length();break; + case eStatus::Inside: m_distance=0;break; + default: {} + } + return(m_status); + } + bool EncloseOrigin() + { + switch(m_simplex->rank) + { + case 1: + { + for(U i=0;i<3;++i) + { + Vector3 axis=Vector3(0,0,0); + axis[i]=1; + appendvertice(*m_simplex, axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-axis); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 2: + { + const Vector3 d=m_simplex->c[1]->w-m_simplex->c[0]->w; + for(U i=0;i<3;++i) + { + Vector3 axis=Vector3(0,0,0); + axis[i]=1; + const Vector3 p=vec3_cross(d,axis); + if(p.length_squared()>0) + { + appendvertice(*m_simplex, p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-p); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + } + break; + case 3: + { + const Vector3 n=vec3_cross(m_simplex->c[1]->w-m_simplex->c[0]->w, + m_simplex->c[2]->w-m_simplex->c[0]->w); + if(n.length_squared()>0) + { + appendvertice(*m_simplex,n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + appendvertice(*m_simplex,-n); + if(EncloseOrigin()) return(true); + removevertice(*m_simplex); + } + } + break; + case 4: + { + if(Math::abs(det( m_simplex->c[0]->w-m_simplex->c[3]->w, + m_simplex->c[1]->w-m_simplex->c[3]->w, + m_simplex->c[2]->w-m_simplex->c[3]->w))>0) + return(true); + } + break; + } + return(false); + } + /* Internals */ + void getsupport(const Vector3& d,sSV& sv) const + { + sv.d = d/d.length(); + sv.w = m_shape.Support(sv.d); + } + void removevertice(sSimplex& simplex) + { + m_free[m_nfree++]=simplex.c[--simplex.rank]; + } + void appendvertice(sSimplex& simplex,const Vector3& v) + { + simplex.p[simplex.rank]=0; + simplex.c[simplex.rank]=m_free[--m_nfree]; + getsupport(v,*simplex.c[simplex.rank++]); + } + static real_t det(const Vector3& a,const Vector3& b,const Vector3& c) + { + return( a.y*b.z*c.x+a.z*b.x*c.y- + a.x*b.z*c.y-a.y*b.x*c.z+ + a.x*b.y*c.z-a.z*b.y*c.x); + } + static real_t projectorigin( const Vector3& a, + const Vector3& b, + real_t* w,U& m) + { + const Vector3 d=b-a; + const real_t l=d.length_squared(); + if(l>GJK_SIMPLEX2_EPS) + { + const real_t t(l>0?-vec3_dot(a,d)/l:0); + if(t>=1) { w[0]=0;w[1]=1;m=2;return(b.length_squared()); } + else if(t<=0) { w[0]=1;w[1]=0;m=1;return(a.length_squared()); } + else { w[0]=1-(w[1]=t);m=3;return((a+d*t).length_squared()); } + } + return(-1); + } + static real_t projectorigin( const Vector3& a, + const Vector3& b, + const Vector3& c, + real_t* w,U& m) + { + static const U imd3[]={1,2,0}; + const Vector3* vt[]={&a,&b,&c}; + const Vector3 dl[]={a-b,b-c,c-a}; + const Vector3 n=vec3_cross(dl[0],dl[1]); + const real_t l=n.length_squared(); + if(l>GJK_SIMPLEX3_EPS) + { + real_t mindist=-1; + real_t subw[2]; + U subm; + for(U i=0;i<3;++i) + { + if(vec3_dot(*vt[i],vec3_cross(dl[i],n))>0) + { + const U j=imd3[i]; + const real_t subd(projectorigin(*vt[i],*vt[j],subw,subm)); + if((mindist<0)||(subd<mindist)) + { + mindist = subd; + m = static_cast<U>(((subm&1)?1<<i:0)+((subm&2)?1<<j:0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + } + } + } + if(mindist<0) + { + const real_t d=vec3_dot(a,n); + const real_t s=Math::sqrt(l); + const Vector3 p=n*(d/l); + mindist = p.length_squared(); + m = 7; + w[0] = (vec3_cross(dl[1],b-p)).length()/s; + w[1] = (vec3_cross(dl[2],c-p)).length()/s; + w[2] = 1-(w[0]+w[1]); + } + return(mindist); + } + return(-1); + } + static real_t projectorigin( const Vector3& a, + const Vector3& b, + const Vector3& c, + const Vector3& d, + real_t* w,U& m) + { + static const U imd3[]={1,2,0}; + const Vector3* vt[]={&a,&b,&c,&d}; + const Vector3 dl[]={a-d,b-d,c-d}; + const real_t vl=det(dl[0],dl[1],dl[2]); + const bool ng=(vl*vec3_dot(a,vec3_cross(b-c,a-b)))<=0; + if(ng&&(Math::abs(vl)>GJK_SIMPLEX4_EPS)) + { + real_t mindist=-1; + real_t subw[3]; + U subm; + for(U i=0;i<3;++i) + { + const U j=imd3[i]; + const real_t s=vl*vec3_dot(d,vec3_cross(dl[i],dl[j])); + if(s>0) + { + const real_t subd=projectorigin(*vt[i],*vt[j],d,subw,subm); + if((mindist<0)||(subd<mindist)) + { + mindist = subd; + m = static_cast<U>((subm&1?1<<i:0)+ + (subm&2?1<<j:0)+ + (subm&4?8:0)); + w[i] = subw[0]; + w[j] = subw[1]; + w[imd3[j]] = 0; + w[3] = subw[2]; + } + } + } + if(mindist<0) + { + mindist = 0; + m = 15; + w[0] = det(c,b,d)/vl; + w[1] = det(a,c,d)/vl; + w[2] = det(b,a,d)/vl; + w[3] = 1-(w[0]+w[1]+w[2]); + } + return(mindist); + } + return(-1); + } +}; + + // EPA + struct EPA + { + /* Types */ + typedef GJK::sSV sSV; + struct sFace + { + Vector3 n; + real_t d; + real_t p; + sSV* c[3]; + sFace* f[3]; + sFace* l[2]; + U1 e[3]; + U1 pass; + }; + struct sList + { + sFace* root; + U count; + sList() : root(0),count(0) {} + }; + struct sHorizon + { + sFace* cf; + sFace* ff; + U nf; + sHorizon() : cf(0),ff(0),nf(0) {} + }; + struct eStatus { enum _ { + Valid, + Touching, + Degenerated, + NonConvex, + InvalidHull, + OutOfFaces, + OutOfVertices, + AccuraryReached, + FallBack, + Failed };}; + /* Fields */ + eStatus::_ m_status; + GJK::sSimplex m_result; + Vector3 m_normal; + real_t m_depth; + sSV m_sv_store[EPA_MAX_VERTICES]; + sFace m_fc_store[EPA_MAX_FACES]; + U m_nextsv; + sList m_hull; + sList m_stock; + /* Methods */ + EPA() + { + Initialize(); + } + + + static inline void bind(sFace* fa,U ea,sFace* fb,U eb) + { + fa->e[ea]=(U1)eb;fa->f[ea]=fb; + fb->e[eb]=(U1)ea;fb->f[eb]=fa; + } + static inline void append(sList& list,sFace* face) + { + face->l[0] = 0; + face->l[1] = list.root; + if(list.root) list.root->l[0]=face; + list.root = face; + ++list.count; + } + static inline void remove(sList& list,sFace* face) + { + if(face->l[1]) face->l[1]->l[0]=face->l[0]; + if(face->l[0]) face->l[0]->l[1]=face->l[1]; + if(face==list.root) list.root=face->l[1]; + --list.count; + } + + + void Initialize() + { + m_status = eStatus::Failed; + m_normal = Vector3(0,0,0); + m_depth = 0; + m_nextsv = 0; + for(U i=0;i<EPA_MAX_FACES;++i) + { + append(m_stock,&m_fc_store[EPA_MAX_FACES-i-1]); + } + } + eStatus::_ Evaluate(GJK& gjk,const Vector3& guess) + { + GJK::sSimplex& simplex=*gjk.m_simplex; + if((simplex.rank>1)&&gjk.EncloseOrigin()) + { + + /* Clean up */ + while(m_hull.root) + { + sFace* f = m_hull.root; + remove(m_hull,f); + append(m_stock,f); + } + m_status = eStatus::Valid; + m_nextsv = 0; + /* Orient simplex */ + if(gjk.det( simplex.c[0]->w-simplex.c[3]->w, + simplex.c[1]->w-simplex.c[3]->w, + simplex.c[2]->w-simplex.c[3]->w)<0) + { + SWAP(simplex.c[0],simplex.c[1]); + SWAP(simplex.p[0],simplex.p[1]); + } + /* Build initial hull */ + sFace* tetra[]={newface(simplex.c[0],simplex.c[1],simplex.c[2],true), + newface(simplex.c[1],simplex.c[0],simplex.c[3],true), + newface(simplex.c[2],simplex.c[1],simplex.c[3],true), + newface(simplex.c[0],simplex.c[2],simplex.c[3],true)}; + if(m_hull.count==4) + { + sFace* best=findbest(); + sFace outer=*best; + U pass=0; + U iterations=0; + bind(tetra[0],0,tetra[1],0); + bind(tetra[0],1,tetra[2],0); + bind(tetra[0],2,tetra[3],0); + bind(tetra[1],1,tetra[3],2); + bind(tetra[1],2,tetra[2],1); + bind(tetra[2],2,tetra[3],1); + m_status=eStatus::Valid; + for(;iterations<EPA_MAX_ITERATIONS;++iterations) + { + if(m_nextsv<EPA_MAX_VERTICES) + { + sHorizon horizon; + sSV* w=&m_sv_store[m_nextsv++]; + bool valid=true; + best->pass = (U1)(++pass); + gjk.getsupport(best->n,*w); + const real_t wdist=vec3_dot(best->n,w->w)-best->d; + if(wdist>EPA_ACCURACY) + { + for(U j=0;(j<3)&&valid;++j) + { + valid&=expand( pass,w, + best->f[j],best->e[j], + horizon); + } + if(valid&&(horizon.nf>=3)) + { + bind(horizon.cf,1,horizon.ff,2); + remove(m_hull,best); + append(m_stock,best); + best=findbest(); + if(best->p>=outer.p) outer=*best; + } else { m_status=eStatus::InvalidHull;break; } + } else { m_status=eStatus::AccuraryReached;break; } + } else { m_status=eStatus::OutOfVertices;break; } + } + const Vector3 projection=outer.n*outer.d; + m_normal = outer.n; + m_depth = outer.d; + m_result.rank = 3; + m_result.c[0] = outer.c[0]; + m_result.c[1] = outer.c[1]; + m_result.c[2] = outer.c[2]; + m_result.p[0] = vec3_cross( outer.c[1]->w-projection, + outer.c[2]->w-projection).length(); + m_result.p[1] = vec3_cross( outer.c[2]->w-projection, + outer.c[0]->w-projection).length(); + m_result.p[2] = vec3_cross( outer.c[0]->w-projection, + outer.c[1]->w-projection).length(); + const real_t sum=m_result.p[0]+m_result.p[1]+m_result.p[2]; + m_result.p[0] /= sum; + m_result.p[1] /= sum; + m_result.p[2] /= sum; + return(m_status); + } + } + /* Fallback */ + m_status = eStatus::FallBack; + m_normal = -guess; + const real_t nl=m_normal.length(); + if(nl>0) + m_normal = m_normal/nl; + else + m_normal = Vector3(1,0,0); + m_depth = 0; + m_result.rank=1; + m_result.c[0]=simplex.c[0]; + m_result.p[0]=1; + return(m_status); + } + sFace* newface(sSV* a,sSV* b,sSV* c,bool forced) + { + if(m_stock.root) + { + sFace* face=m_stock.root; + remove(m_stock,face); + append(m_hull,face); + face->pass = 0; + face->c[0] = a; + face->c[1] = b; + face->c[2] = c; + face->n = vec3_cross(b->w-a->w,c->w-a->w); + const real_t l=face->n.length(); + const bool v=l>EPA_ACCURACY; + face->p = MIN(MIN( + vec3_dot(a->w,vec3_cross(face->n,a->w-b->w)), + vec3_dot(b->w,vec3_cross(face->n,b->w-c->w))), + vec3_dot(c->w,vec3_cross(face->n,c->w-a->w))) / + (v?l:1); + face->p = face->p>=-EPA_INSIDE_EPS?0:face->p; + if(v) + { + face->d = vec3_dot(a->w,face->n)/l; + face->n /= l; + if(forced||(face->d>=-EPA_PLANE_EPS)) + { + return(face); + } else m_status=eStatus::NonConvex; + } else m_status=eStatus::Degenerated; + remove(m_hull,face); + append(m_stock,face); + return(0); + } + m_status=m_stock.root?eStatus::OutOfVertices:eStatus::OutOfFaces; + return(0); + } + sFace* findbest() + { + sFace* minf=m_hull.root; + real_t mind=minf->d*minf->d; + real_t maxp=minf->p; + for(sFace* f=minf->l[1];f;f=f->l[1]) + { + const real_t sqd=f->d*f->d; + if((f->p>=maxp)&&(sqd<mind)) + { + minf=f; + mind=sqd; + maxp=f->p; + } + } + return(minf); + } + bool expand(U pass,sSV* w,sFace* f,U e,sHorizon& horizon) + { + static const U i1m3[]={1,2,0}; + static const U i2m3[]={2,0,1}; + if(f->pass!=pass) + { + const U e1=i1m3[e]; + if((vec3_dot(f->n,w->w)-f->d)<-EPA_PLANE_EPS) + { + sFace* nf=newface(f->c[e1],f->c[e],w,false); + if(nf) + { + bind(nf,0,f,e); + if(horizon.cf) bind(horizon.cf,1,nf,2); else horizon.ff=nf; + horizon.cf=nf; + ++horizon.nf; + return(true); + } + } + else + { + const U e2=i2m3[e]; + f->pass = (U1)pass; + if( expand(pass,w,f->f[e1],f->e[e1],horizon)&& + expand(pass,w,f->f[e2],f->e[e2],horizon)) + { + remove(m_hull,f); + append(m_stock,f); + return(true); + } + } + } + return(false); + } + + }; + + // + static void Initialize( const ShapeSW* shape0,const Transform& wtrs0, + const ShapeSW* shape1,const Transform& wtrs1, + sResults& results, + tShape& shape, + bool withmargins) + { + /* Results */ + results.witnesses[0] = + results.witnesses[1] = Vector3(0,0,0); + results.status = sResults::Separated; + /* Shape */ + shape.m_shapes[0] = shape0; + shape.m_shapes[1] = shape1; + shape.transform_A = wtrs0; + shape.transform_B = wtrs1; + + } + + + +// +// Api +// + +// + +// +bool Distance( const ShapeSW* shape0, + const Transform& wtrs0, + const ShapeSW* shape1, + const Transform& wtrs1, + const Vector3& guess, + sResults& results) +{ + tShape shape; + Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); + GJK gjk; + GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,guess); + if(gjk_status==GJK::eStatus::Valid) + { + Vector3 w0=Vector3(0,0,0); + Vector3 w1=Vector3(0,0,0); + for(U i=0;i<gjk.m_simplex->rank;++i) + { + const real_t p=gjk.m_simplex->p[i]; + w0+=shape.Support( gjk.m_simplex->c[i]->d,0)*p; + w1+=shape.Support(-gjk.m_simplex->c[i]->d,1)*p; + } + results.witnesses[0] = wtrs0.xform(w0); + results.witnesses[1] = wtrs0.xform(w1); + results.normal = w0-w1; + results.distance = results.normal.length(); + results.normal /= results.distance>GJK_MIN_DISTANCE?results.distance:1; + return(true); + } + else + { + results.status = gjk_status==GJK::eStatus::Inside? + sResults::Penetrating : + sResults::GJK_Failed ; + return(false); + } +} + +// +bool Penetration( const ShapeSW* shape0, + const Transform& wtrs0, + const ShapeSW* shape1, + const Transform& wtrs1, + const Vector3& guess, + sResults& results + ) +{ + tShape shape; + Initialize(shape0,wtrs0,shape1,wtrs1,results,shape,false); + GJK gjk; + GJK::eStatus::_ gjk_status=gjk.Evaluate(shape,-guess); + switch(gjk_status) + { + case GJK::eStatus::Inside: + { + EPA epa; + EPA::eStatus::_ epa_status=epa.Evaluate(gjk,-guess); + if(epa_status!=EPA::eStatus::Failed) + { + Vector3 w0=Vector3(0,0,0); + for(U i=0;i<epa.m_result.rank;++i) + { + w0+=shape.Support(epa.m_result.c[i]->d,0)*epa.m_result.p[i]; + } + results.status = sResults::Penetrating; + results.witnesses[0] = w0; + results.witnesses[1] = w0-epa.m_normal*epa.m_depth; + results.normal = -epa.m_normal; + results.distance = -epa.m_depth; + return(true); + } else results.status=sResults::EPA_Failed; + } + break; + case GJK::eStatus::Failed: + results.status=sResults::GJK_Failed; + break; + default: {} + } + return(false); +} + + +/* Symbols cleanup */ + +#undef GJK_MAX_ITERATIONS +#undef GJK_ACCURARY +#undef GJK_MIN_DISTANCE +#undef GJK_DUPLICATED_EPS +#undef GJK_SIMPLEX2_EPS +#undef GJK_SIMPLEX3_EPS +#undef GJK_SIMPLEX4_EPS + +#undef EPA_MAX_VERTICES +#undef EPA_MAX_FACES +#undef EPA_MAX_ITERATIONS +#undef EPA_ACCURACY +#undef EPA_FALLBACK +#undef EPA_PLANE_EPS +#undef EPA_INSIDE_EPS + + +} // end of namespace + + + +bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap ) { + + GjkEpa2::sResults res; + + if (GjkEpa2::Penetration(p_shape_A,p_transform_A,p_shape_B,p_transform_B,p_transform_B.origin-p_transform_A.origin,res)) { + if (p_result_callback) { + if (p_swap) + p_result_callback(res.witnesses[1],res.witnesses[0],p_userdata); + else + p_result_callback(res.witnesses[0],res.witnesses[1],p_userdata); + } + return true; + } + + return false; +} + diff --git a/servers/physics/gjk_epa.h b/servers/physics/gjk_epa.h new file mode 100644 index 0000000000..0303478f17 --- /dev/null +++ b/servers/physics/gjk_epa.h @@ -0,0 +1,40 @@ +/*************************************************************************/ +/* gjk_epa.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef GJK_EPA_H +#define GJK_EPA_H + +#include "shape_sw.h" +/** + @author Juan Linietsky <reduzio@gmail.com> +*/ +#include "collision_solver_sw.h" + +bool gjk_epa_calculate_penetration(const ShapeSW *p_shape_A, const Transform& p_transform_A, const ShapeSW *p_shape_B, const Transform& p_transform_B, CollisionSolverSW::CallbackResult p_result_callback,void *p_userdata, bool p_swap=false); + +#endif diff --git a/servers/physics/joints_sw.cpp b/servers/physics/joints_sw.cpp new file mode 100644 index 0000000000..f9e22e1665 --- /dev/null +++ b/servers/physics/joints_sw.cpp @@ -0,0 +1,450 @@ +/*************************************************************************/ +/* joints_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "joints_sw.h" +#include "space_sw.h" + +#if 0 + +//based on chipmunk joint constraints + +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +static inline real_t k_scalar(Body2DSW *a,Body2DSW *b,const Vector2& rA, const Vector2& rB, const Vector2& n) { + + + real_t value=0; + + + { + value+=a->get_inv_mass(); + real_t rcn = rA.cross(n); + value+=a->get_inv_inertia() * rcn * rcn; + } + + if (b) { + + value+=b->get_inv_mass(); + real_t rcn = rB.cross(n); + value+=b->get_inv_inertia() * rcn * rcn; + } + + return value; + +} + + +bool PinJoint2DSW::setup(float p_step) { + + Space2DSW *space = A->get_space(); + ERR_FAIL_COND_V(!space,false;) + rA = A->get_transform().xform(anchor_A); + rB = B?B->get_transform().xform(anchor_B):anchor_B; + + Vector2 delta = rB - rA; + + rA-= A->get_transform().get_origin(); + if (B) + rB-=B->get_transform().get_origin(); + + + real_t jdist = delta.length(); + correct=false; + if (jdist==0) + return false; // do not correct + + correct=true; + + n = delta / jdist; + + // calculate mass normal + mass_normal = 1.0f/k_scalar(A, B, rA, rB, n); + + // calculate bias velocity + //real_t maxBias = joint->constraint.maxBias; + bias = -(get_bias()==0?space->get_constraint_bias():get_bias())*(1.0/p_step)*(jdist-dist); + bias = CLAMP(bias, -get_max_bias(), +get_max_bias()); + + // compute max impulse + jn_max = get_max_force() * p_step; + + // apply accumulated impulse + Vector2 j = n * jn_acc; + A->apply_impulse(rA,-j); + if (B) + B->apply_impulse(rB,j); + + return true; +} + + +static inline Vector2 +relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB){ + Vector2 sum = a->get_linear_velocity() -rA.tangent() * a->get_angular_velocity(); + if (b) + return (b->get_linear_velocity() -rB.tangent() * b->get_angular_velocity()) - sum; + else + return -sum; +} + +static inline real_t +normal_relative_velocity(Body2DSW *a, Body2DSW *b, Vector2 rA, Vector2 rB, Vector2 n){ + return relative_velocity(a, b, rA, rB).dot(n); +} + + +void PinJoint2DSW::solve(float p_step){ + + if (!correct) + return; + + Vector2 ln = n; + + // compute relative velocity + real_t vrn = normal_relative_velocity(A,B, rA, rB, ln); + + // compute normal impulse + real_t jn = (bias - vrn)*mass_normal; + real_t jnOld = jn_acc; + jn_acc = CLAMP(jnOld + jn,-jn_max,jn_max); //cpfclamp(jnOld + jn, -joint->jnMax, joint->jnMax); + jn = jn_acc - jnOld; + + Vector2 j = jn*ln; + + A->apply_impulse(rA,-j); + if (B) + B->apply_impulse(rB,j); + +} + + +PinJoint2DSW::PinJoint2DSW(const Vector2& p_pos,Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,p_body_b?2:1) { + + A=p_body_a; + B=p_body_b; + anchor_A = p_body_a->get_inv_transform().xform(p_pos); + anchor_B = p_body_b?p_body_b->get_inv_transform().xform(p_pos):p_pos; + + jn_acc=0; + dist=0; + + p_body_a->add_constraint(this,0); + if (p_body_b) + p_body_b->add_constraint(this,1); + +} + +PinJoint2DSW::~PinJoint2DSW() { + + if (A) + A->remove_constraint(this); + if (B) + B->remove_constraint(this); + +} + +////////////////////////////////////////////// +////////////////////////////////////////////// +////////////////////////////////////////////// + + +static inline void +k_tensor(Body2DSW *a, Body2DSW *b, Vector2 r1, Vector2 r2, Vector2 *k1, Vector2 *k2) +{ + // calculate mass matrix + // If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross... + real_t k11, k12, k21, k22; + real_t m_sum = a->get_inv_mass() + b->get_inv_mass(); + + // start with I*m_sum + k11 = m_sum; k12 = 0.0f; + k21 = 0.0f; k22 = m_sum; + + // add the influence from r1 + real_t a_i_inv = a->get_inv_inertia(); + real_t r1xsq = r1.x * r1.x * a_i_inv; + real_t r1ysq = r1.y * r1.y * a_i_inv; + real_t r1nxy = -r1.x * r1.y * a_i_inv; + k11 += r1ysq; k12 += r1nxy; + k21 += r1nxy; k22 += r1xsq; + + // add the influnce from r2 + real_t b_i_inv = b->get_inv_inertia(); + real_t r2xsq = r2.x * r2.x * b_i_inv; + real_t r2ysq = r2.y * r2.y * b_i_inv; + real_t r2nxy = -r2.x * r2.y * b_i_inv; + k11 += r2ysq; k12 += r2nxy; + k21 += r2nxy; k22 += r2xsq; + + // invert + real_t determinant = k11*k22 - k12*k21; + ERR_FAIL_COND(determinant== 0.0); + + real_t det_inv = 1.0f/determinant; + *k1 = Vector2( k22*det_inv, -k12*det_inv); + *k2 = Vector2(-k21*det_inv, k11*det_inv); +} + +static _FORCE_INLINE_ Vector2 +mult_k(const Vector2& vr, const Vector2 &k1, const Vector2 &k2) +{ + return Vector2(vr.dot(k1), vr.dot(k2)); +} + +bool GrooveJoint2DSW::setup(float p_step) { + + + // calculate endpoints in worldspace + Vector2 ta = A->get_transform().xform(A_groove_1); + Vector2 tb = A->get_transform().xform(A_groove_2); + Space2DSW *space=A->get_space(); + + // calculate axis + Vector2 n = -(tb - ta).tangent().normalized(); + real_t d = ta.dot(n); + + xf_normal = n; + rB = B->get_transform().basis_xform(B_anchor); + + // calculate tangential distance along the axis of rB + real_t td = (B->get_transform().get_origin() + rB).cross(n); + // calculate clamping factor and rB + if(td <= ta.cross(n)){ + clamp = 1.0f; + rA = ta - A->get_transform().get_origin(); + } else if(td >= tb.cross(n)){ + clamp = -1.0f; + rA = tb - A->get_transform().get_origin(); + } else { + clamp = 0.0f; + //joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p); + rA = ((-n.tangent() * -td) + n*d) - A->get_transform().get_origin(); + } + + // Calculate mass tensor + k_tensor(A, B, rA, rB, &k1, &k2); + + // compute max impulse + jn_max = get_max_force() * p_step; + + // calculate bias velocity +// cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); +// joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias); + + + Vector2 delta = (B->get_transform().get_origin() +rB) - (A->get_transform().get_origin() + rA); + gbias=(delta*-(get_bias()==0?space->get_constraint_bias():get_bias())*(1.0/p_step)).clamped(get_max_bias()); + + // apply accumulated impulse + A->apply_impulse(rA,-jn_acc); + B->apply_impulse(rB,jn_acc); + + correct=true; + return true; +} + +void GrooveJoint2DSW::solve(float p_step){ + + + // compute impulse + Vector2 vr = relative_velocity(A, B, rA,rB); + + Vector2 j = mult_k(gbias-vr, k1, k2); + Vector2 jOld = jn_acc; + j+=jOld; + + jn_acc = (((clamp * j.cross(xf_normal)) > 0) ? j : xf_normal.project(j)).clamped(jn_max); + + j = jn_acc - jOld; + + A->apply_impulse(rA,-j); + B->apply_impulse(rB,j); +} + + +GrooveJoint2DSW::GrooveJoint2DSW(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,2) { + + A=p_body_a; + B=p_body_b; + + A_groove_1 = A->get_inv_transform().xform(p_a_groove1); + A_groove_2 = A->get_inv_transform().xform(p_a_groove2); + B_anchor=B->get_inv_transform().xform(p_b_anchor); + A_groove_normal = -(A_groove_2 - A_groove_1).normalized().tangent(); + + A->add_constraint(this,0); + B->add_constraint(this,1); + +} + +GrooveJoint2DSW::~GrooveJoint2DSW() { + + A->remove_constraint(this); + B->remove_constraint(this); +} + + +////////////////////////////////////////////// +////////////////////////////////////////////// +////////////////////////////////////////////// + + +bool DampedSpringJoint2DSW::setup(float p_step) { + + rA = A->get_transform().basis_xform(anchor_A); + rB = B->get_transform().basis_xform(anchor_B); + + Vector2 delta = (B->get_transform().get_origin() + rB) - (A->get_transform().get_origin() + rA) ; + real_t dist = delta.length(); + + if (dist) + n=delta/dist; + else + n=Vector2(); + + real_t k = k_scalar(A, B, rA, rB, n); + n_mass = 1.0f/k; + + target_vrn = 0.0f; + v_coef = 1.0f - Math::exp(-damping*(p_step)*k); + + // apply spring force + real_t f_spring = (rest_length - dist) * stiffness; + Vector2 j = n * f_spring*(p_step); + + A->apply_impulse(rA,-j); + B->apply_impulse(rB,j); + + + return true; +} + +void DampedSpringJoint2DSW::solve(float p_step) { + + // compute relative velocity + real_t vrn = normal_relative_velocity(A, B, rA, rB, n) - target_vrn; + + // compute velocity loss from drag + // not 100% certain this is derived correctly, though it makes sense + real_t v_damp = -vrn*v_coef; + target_vrn = vrn + v_damp; + Vector2 j=n*v_damp*n_mass; + + A->apply_impulse(rA,-j); + B->apply_impulse(rB,j); + +} + +void DampedSpringJoint2DSW::set_param(Physics2DServer::DampedStringParam p_param, real_t p_value) { + + switch(p_param) { + + case Physics2DServer::DAMPED_STRING_REST_LENGTH: { + + rest_length=p_value; + } break; + case Physics2DServer::DAMPED_STRING_DAMPING: { + + damping=p_value; + } break; + case Physics2DServer::DAMPED_STRING_STIFFNESS: { + + stiffness=p_value; + } break; + } + +} + +real_t DampedSpringJoint2DSW::get_param(Physics2DServer::DampedStringParam p_param) const{ + + switch(p_param) { + + case Physics2DServer::DAMPED_STRING_REST_LENGTH: { + + return rest_length; + } break; + case Physics2DServer::DAMPED_STRING_DAMPING: { + + return damping; + } break; + case Physics2DServer::DAMPED_STRING_STIFFNESS: { + + return stiffness; + } break; + } + + ERR_FAIL_V(0); +} + + +DampedSpringJoint2DSW::DampedSpringJoint2DSW(const Vector2& p_anchor_a,const Vector2& p_anchor_b, Body2DSW* p_body_a,Body2DSW* p_body_b) : Joint2DSW(_arr,2) { + + + A=p_body_a; + B=p_body_b; + anchor_A = A->get_inv_transform().xform(p_anchor_a); + anchor_B = B->get_inv_transform().xform(p_anchor_b); + + rest_length=p_anchor_a.distance_to(p_anchor_b); + stiffness=20; + damping=1.5; + + + A->add_constraint(this,0); + B->add_constraint(this,1); + +} + +DampedSpringJoint2DSW::~DampedSpringJoint2DSW() { + + A->remove_constraint(this); + B->remove_constraint(this); + +} + + +#endif diff --git a/servers/physics/joints_sw.h b/servers/physics/joints_sw.h new file mode 100644 index 0000000000..c10568fb52 --- /dev/null +++ b/servers/physics/joints_sw.h @@ -0,0 +1,176 @@ +/*************************************************************************/ +/* joints_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef JOINTS_SW_H +#define JOINTS_SW_H + +#include "constraint_sw.h" +#include "body_sw.h" + + + +class JointSW : public ConstraintSW { + + real_t max_force; + real_t bias; + real_t max_bias; +public: + + _FORCE_INLINE_ void set_max_force(real_t p_force) { max_force=p_force; } + _FORCE_INLINE_ real_t get_max_force() const { return max_force; } + + _FORCE_INLINE_ void set_bias(real_t p_bias) { bias=p_bias; } + _FORCE_INLINE_ real_t get_bias() const { return bias; } + + _FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias=p_bias; } + _FORCE_INLINE_ real_t get_max_bias() const { return max_bias; } + +// virtual PhysicsServer::JointType get_type() const=0; + JointSW(BodySW **p_body_ptr=NULL,int p_body_count=0) : ConstraintSW(p_body_ptr,p_body_count) { bias=0; max_force=max_bias=3.40282e+38; }; + +}; + +#if 0 +class PinJointSW : public JointSW { + + union { + struct { + BodySW *A; + BodySW *B; + }; + + BodySW *_arr[2]; + }; + + Vector2 anchor_A; + Vector2 anchor_B; + real_t dist; + real_t jn_acc; + real_t jn_max; + real_t max_distance; + real_t mass_normal; + real_t bias; + + Vector2 rA,rB; + Vector2 n; //normal + bool correct; + + +public: + + virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_PIN; } + + virtual bool setup(float p_step); + virtual void solve(float p_step); + + + PinJointSW(const Vector2& p_pos,BodySW* p_body_a,BodySW* p_body_b=NULL); + ~PinJointSW(); +}; + + +class GrooveJointSW : public JointSW { + + union { + struct { + BodySW *A; + BodySW *B; + }; + + BodySW *_arr[2]; + }; + + Vector2 A_groove_1; + Vector2 A_groove_2; + Vector2 A_groove_normal; + Vector2 B_anchor; + Vector2 jn_acc; + Vector2 gbias; + real_t jn_max; + real_t clamp; + Vector2 xf_normal; + Vector2 rA,rB; + Vector2 k1,k2; + + + bool correct; + +public: + + virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_GROOVE; } + + virtual bool setup(float p_step); + virtual void solve(float p_step); + + + GrooveJointSW(const Vector2& p_a_groove1,const Vector2& p_a_groove2, const Vector2& p_b_anchor, BodySW* p_body_a,BodySW* p_body_b); + ~GrooveJointSW(); +}; + + +class DampedSpringJointSW : public JointSW { + + union { + struct { + BodySW *A; + BodySW *B; + }; + + BodySW *_arr[2]; + }; + + + Vector2 anchor_A; + Vector2 anchor_B; + + real_t rest_length; + real_t damping; + real_t stiffness; + + Vector2 rA,rB; + Vector2 n; + real_t n_mass; + real_t target_vrn; + real_t v_coef; + +public: + + virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_DAMPED_SPRING; } + + virtual bool setup(float p_step); + virtual void solve(float p_step); + + void set_param(PhysicsServer::DampedStringParam p_param, real_t p_value); + real_t get_param(PhysicsServer::DampedStringParam p_param) const; + + DampedSpringJointSW(const Vector2& p_anchor_a,const Vector2& p_anchor_b, BodySW* p_body_a,BodySW* p_body_b); + ~DampedSpringJointSW(); +}; +#endif + +#endif // JOINTS__SW_H diff --git a/servers/physics/physics_server_sw.cpp b/servers/physics/physics_server_sw.cpp new file mode 100644 index 0000000000..072f11aa52 --- /dev/null +++ b/servers/physics/physics_server_sw.cpp @@ -0,0 +1,1050 @@ +/*************************************************************************/ +/* physics_server_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "physics_server_sw.h" +#include "broad_phase_basic.h" +#include "broad_phase_octree.h" + +RID PhysicsServerSW::shape_create(ShapeType p_shape) { + + ShapeSW *shape=NULL; + switch(p_shape) { + + case SHAPE_PLANE: { + + shape=memnew( PlaneShapeSW ); + } break; + case SHAPE_RAY: { + + shape=memnew( RayShapeSW ); + } break; + case SHAPE_SPHERE: { + + shape=memnew( SphereShapeSW); + } break; + case SHAPE_BOX: { + + shape=memnew( BoxShapeSW); + } break; + case SHAPE_CAPSULE: { + + shape=memnew( CapsuleShapeSW ); + } break; + case SHAPE_CONVEX_POLYGON: { + + shape=memnew( ConvexPolygonShapeSW ); + } break; + case SHAPE_CONCAVE_POLYGON: { + + shape=memnew( ConcavePolygonShapeSW ); + } break; + case SHAPE_HEIGHTMAP: { + + shape=memnew( HeightMapShapeSW ); + } break; + case SHAPE_CUSTOM: { + + ERR_FAIL_V(RID()); + + } break; + + } + + RID id = shape_owner.make_rid(shape); + shape->set_self(id); + + return id; +}; + +void PhysicsServerSW::shape_set_data(RID p_shape, const Variant& p_data) { + + ShapeSW *shape = shape_owner.get(p_shape); + ERR_FAIL_COND(!shape); + shape->set_data(p_data); + + +}; + + +void PhysicsServerSW::shape_set_custom_solver_bias(RID p_shape, real_t p_bias) { + + ShapeSW *shape = shape_owner.get(p_shape); + ERR_FAIL_COND(!shape); + shape->set_custom_bias(p_bias); + +} + + +PhysicsServer::ShapeType PhysicsServerSW::shape_get_type(RID p_shape) const { + + const ShapeSW *shape = shape_owner.get(p_shape); + ERR_FAIL_COND_V(!shape,SHAPE_CUSTOM); + return shape->get_type(); + +}; + +Variant PhysicsServerSW::shape_get_data(RID p_shape) const { + + const ShapeSW *shape = shape_owner.get(p_shape); + ERR_FAIL_COND_V(!shape,Variant()); + ERR_FAIL_COND_V(!shape->is_configured(),Variant()); + return shape->get_data(); + +}; + +real_t PhysicsServerSW::shape_get_custom_solver_bias(RID p_shape) const { + + const ShapeSW *shape = shape_owner.get(p_shape); + ERR_FAIL_COND_V(!shape,0); + return shape->get_custom_bias(); + +} + + +RID PhysicsServerSW::space_create() { + + SpaceSW *space = memnew( SpaceSW ); + RID id = space_owner.make_rid(space); + space->set_self(id); + RID area_id = area_create(); + AreaSW *area = area_owner.get(area_id); + ERR_FAIL_COND_V(!area,RID()); + space->set_default_area(area); + area->set_space(space); + area->set_priority(-1); + + return id; +}; + +void PhysicsServerSW::space_set_active(RID p_space,bool p_active) { + + SpaceSW *space = space_owner.get(p_space); + ERR_FAIL_COND(!space); + if (p_active) + active_spaces.insert(space); + else + active_spaces.erase(space); +} + +bool PhysicsServerSW::space_is_active(RID p_space) const { + + const SpaceSW *space = space_owner.get(p_space); + ERR_FAIL_COND_V(!space,false); + + return active_spaces.has(space); + +} + +void PhysicsServerSW::space_set_param(RID p_space,SpaceParameter p_param, real_t p_value) { + + SpaceSW *space = space_owner.get(p_space); + ERR_FAIL_COND(!space); + + space->set_param(p_param,p_value); + +} + +real_t PhysicsServerSW::space_get_param(RID p_space,SpaceParameter p_param) const { + + const SpaceSW *space = space_owner.get(p_space); + ERR_FAIL_COND_V(!space,0); + return space->get_param(p_param); +} + +PhysicsDirectSpaceState* PhysicsServerSW::space_get_direct_state(RID p_space) { + + SpaceSW *space = space_owner.get(p_space); + ERR_FAIL_COND_V(!space,NULL); + if (!doing_sync || space->is_locked()) { + + ERR_EXPLAIN("Space state is inaccesible right now, wait for iteration or fixed process notification."); + ERR_FAIL_V(NULL); + + + } + + return space->get_direct_state(); +} + +RID PhysicsServerSW::area_create() { + + AreaSW *area = memnew( AreaSW ); + RID rid = area_owner.make_rid(area); + area->set_self(rid); + return rid; +}; + +void PhysicsServerSW::area_set_space(RID p_area, RID p_space) { + + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + SpaceSW *space=NULL; + if (p_space.is_valid()) { + space = space_owner.get(p_space); + ERR_FAIL_COND(!space); + } + + area->set_space(space); + +}; + +RID PhysicsServerSW::area_get_space(RID p_area) const { + + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND_V(!area,RID()); + + SpaceSW *space = area->get_space(); + if (!space) + return RID(); + return space->get_self(); +}; + +void PhysicsServerSW::area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode) { + + + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + + area->set_space_override_mode(p_mode); +} + +PhysicsServer::AreaSpaceOverrideMode PhysicsServerSW::area_get_space_override_mode(RID p_area) const { + + const AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND_V(!area,AREA_SPACE_OVERRIDE_DISABLED); + + return area->get_space_override_mode(); +} + + +void PhysicsServerSW::area_add_shape(RID p_area, RID p_shape, const Transform& p_transform) { + + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + + ShapeSW *shape = shape_owner.get(p_shape); + ERR_FAIL_COND(!shape); + + area->add_shape(shape,p_transform); + +} + +void PhysicsServerSW::area_set_shape(RID p_area, int p_shape_idx,RID p_shape) { + + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + + ShapeSW *shape = shape_owner.get(p_shape); + ERR_FAIL_COND(!shape); + ERR_FAIL_COND(!shape->is_configured()); + + area->set_shape(p_shape_idx,shape); + +} +void PhysicsServerSW::area_set_shape_transform(RID p_area, int p_shape_idx, const Transform& p_transform) { + + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + + area->set_shape_transform(p_shape_idx,p_transform); +} + +int PhysicsServerSW::area_get_shape_count(RID p_area) const { + + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND_V(!area,-1); + + return area->get_shape_count(); + +} +RID PhysicsServerSW::area_get_shape(RID p_area, int p_shape_idx) const { + + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND_V(!area,RID()); + + ShapeSW *shape = area->get_shape(p_shape_idx); + ERR_FAIL_COND_V(!shape,RID()); + + return shape->get_self(); +} +Transform PhysicsServerSW::area_get_shape_transform(RID p_area, int p_shape_idx) const { + + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND_V(!area,Transform()); + + return area->get_shape_transform(p_shape_idx); +} + +void PhysicsServerSW::area_remove_shape(RID p_area, int p_shape_idx) { + + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + + area->remove_shape(p_shape_idx); +} + +void PhysicsServerSW::area_clear_shapes(RID p_area) { + + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + + while(area->get_shape_count()) + area->remove_shape(0); + +} + +void PhysicsServerSW::area_attach_object_instance_ID(RID p_area,ObjectID p_ID) { + + if (space_owner.owns(p_area)) { + SpaceSW *space=space_owner.get(p_area); + p_area=space->get_default_area()->get_self(); + } + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + area->set_instance_id(p_ID); + +} +ObjectID PhysicsServerSW::area_get_object_instance_ID(RID p_area) const { + + if (space_owner.owns(p_area)) { + SpaceSW *space=space_owner.get(p_area); + p_area=space->get_default_area()->get_self(); + } + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND_V(!area,0); + return area->get_instance_id(); + + +} + + +void PhysicsServerSW::area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value) { + + if (space_owner.owns(p_area)) { + SpaceSW *space=space_owner.get(p_area); + p_area=space->get_default_area()->get_self(); + } + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + area->set_param(p_param,p_value); + +}; + + +void PhysicsServerSW::area_set_transform(RID p_area, const Transform& p_transform) { + + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + area->set_transform(p_transform); + +}; + +Variant PhysicsServerSW::area_get_param(RID p_area,AreaParameter p_param) const { + + if (space_owner.owns(p_area)) { + SpaceSW *space=space_owner.get(p_area); + p_area=space->get_default_area()->get_self(); + } + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND_V(!area,Variant()); + + return area->get_param(p_param); +}; + +Transform PhysicsServerSW::area_get_transform(RID p_area) const { + + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND_V(!area,Transform()); + + return area->get_transform(); +}; + +void PhysicsServerSW::area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method) { + + AreaSW *area = area_owner.get(p_area); + ERR_FAIL_COND(!area); + + area->set_monitor_callback(p_receiver?p_receiver->get_instance_ID():0,p_method); + + +} + + +/* BODY API */ + +RID PhysicsServerSW::body_create(BodyMode p_mode,bool p_init_sleeping) { + + BodySW *body = memnew( BodySW ); + if (p_mode!=BODY_MODE_RIGID) + body->set_mode(p_mode); + if (p_init_sleeping) + body->set_state(BODY_STATE_SLEEPING,p_init_sleeping); + RID rid = body_owner.make_rid(body); + body->set_self(rid); + return rid; +}; + + +void PhysicsServerSW::body_set_space(RID p_body, RID p_space) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + SpaceSW *space=NULL; + + if (p_space.is_valid()) { + space = space_owner.get(p_space); + ERR_FAIL_COND(!space); + } + + if (body->get_space()==space) + return; //pointles + + body->set_space(space); + +}; + +RID PhysicsServerSW::body_get_space(RID p_body) const { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,RID()); + + SpaceSW *space = body->get_space(); + if (!space) + return RID(); + return space->get_self(); +}; + + +void PhysicsServerSW::body_set_mode(RID p_body, BodyMode p_mode) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_mode(p_mode); +}; + +PhysicsServer::BodyMode PhysicsServerSW::body_get_mode(RID p_body, BodyMode p_mode) const { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,BODY_MODE_STATIC); + + return body->get_mode(); +}; + +void PhysicsServerSW::body_add_shape(RID p_body, RID p_shape, const Transform& p_transform) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + ShapeSW *shape = shape_owner.get(p_shape); + ERR_FAIL_COND(!shape); + + body->add_shape(shape,p_transform); + +} + +void PhysicsServerSW::body_set_shape(RID p_body, int p_shape_idx,RID p_shape) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + ShapeSW *shape = shape_owner.get(p_shape); + ERR_FAIL_COND(!shape); + ERR_FAIL_COND(!shape->is_configured()); + + body->set_shape(p_shape_idx,shape); + +} +void PhysicsServerSW::body_set_shape_transform(RID p_body, int p_shape_idx, const Transform& p_transform) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_shape_transform(p_shape_idx,p_transform); +} + +int PhysicsServerSW::body_get_shape_count(RID p_body) const { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,-1); + + return body->get_shape_count(); + +} +RID PhysicsServerSW::body_get_shape(RID p_body, int p_shape_idx) const { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,RID()); + + ShapeSW *shape = body->get_shape(p_shape_idx); + ERR_FAIL_COND_V(!shape,RID()); + + return shape->get_self(); +} + +void PhysicsServerSW::body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + +} + +bool PhysicsServerSW::body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const{ + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,false); + + // todo ? + + return false; +} + + +Transform PhysicsServerSW::body_get_shape_transform(RID p_body, int p_shape_idx) const { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,Transform()); + + return body->get_shape_transform(p_shape_idx); +} + +void PhysicsServerSW::body_remove_shape(RID p_body, int p_shape_idx) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->remove_shape(p_shape_idx); +} + +void PhysicsServerSW::body_clear_shapes(RID p_body) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + while(body->get_shape_count()) + body->remove_shape(0); + +} + +void PhysicsServerSW::body_set_enable_continuous_collision_detection(RID p_body,bool p_enable) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_continuous_collision_detection(p_enable); + +} + +bool PhysicsServerSW::body_is_continuous_collision_detection_enabled(RID p_body) const { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,false); + + return body->is_continuous_collision_detection_enabled(); +} + +void PhysicsServerSW::body_attach_object_instance_ID(RID p_body,uint32_t p_ID) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_instance_id(p_ID); + +}; + +uint32_t PhysicsServerSW::body_get_object_instance_ID(RID p_body) const { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,0); + + return body->get_instance_id(); +}; + + +void PhysicsServerSW::body_set_user_flags(RID p_body, uint32_t p_flags) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + +}; + +uint32_t PhysicsServerSW::body_get_user_flags(RID p_body, uint32_t p_flags) const { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,0); + + return 0; +}; + +void PhysicsServerSW::body_set_param(RID p_body, BodyParameter p_param, float p_value) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_param(p_param,p_value); +}; + +float PhysicsServerSW::body_get_param(RID p_body, BodyParameter p_param) const { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,0); + + return body->get_param(p_param); +}; + + +void PhysicsServerSW::body_static_simulate_motion(RID p_body,const Transform& p_new_transform) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + body->simulate_motion(p_new_transform,last_step); + +}; + +void PhysicsServerSW::body_set_state(RID p_body, BodyState p_state, const Variant& p_variant) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_state(p_state,p_variant); +}; + +Variant PhysicsServerSW::body_get_state(RID p_body, BodyState p_state) const { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,Variant()); + + return body->get_state(p_state); +}; + + +void PhysicsServerSW::body_set_applied_force(RID p_body, const Vector3& p_force) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_applied_force(p_force); +}; + +Vector3 PhysicsServerSW::body_get_applied_force(RID p_body) const { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,Vector3()); + return body->get_applied_force(); +}; + +void PhysicsServerSW::body_set_applied_torque(RID p_body, const Vector3& p_torque) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_applied_torque(p_torque); +}; + +Vector3 PhysicsServerSW::body_get_applied_torque(RID p_body) const { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,Vector3()); + + return body->get_applied_torque(); +}; + +void PhysicsServerSW::body_apply_impulse(RID p_body, const Vector3& p_pos, const Vector3& p_impulse) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->apply_impulse(p_pos,p_impulse); +}; + +void PhysicsServerSW::body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + Vector3 v = body->get_linear_velocity(); + Vector3 axis = p_axis_velocity.normalized(); + v-=axis*axis.dot(v); + v+=p_axis_velocity; + body->set_linear_velocity(v); + +}; + +void PhysicsServerSW::body_add_collision_exception(RID p_body, RID p_body_b) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->add_exception(p_body_b); + +}; + +void PhysicsServerSW::body_remove_collision_exception(RID p_body, RID p_body_b) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->remove_exception(p_body); + +}; + +void PhysicsServerSW::body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + for(int i=0;i<body->get_exceptions().size();i++) { + p_exceptions->push_back(body->get_exceptions()[i]); + } + +}; + +void PhysicsServerSW::body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + +}; + +float PhysicsServerSW::body_get_contacts_reported_depth_treshold(RID p_body) const { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,0); + return 0; +}; + +void PhysicsServerSW::body_set_omit_force_integration(RID p_body,bool p_omit) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + + body->set_omit_force_integration(p_omit); +}; + +bool PhysicsServerSW::body_is_omitting_force_integration(RID p_body) const { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,false); + return body->get_omit_force_integration(); +}; + +void PhysicsServerSW::body_set_max_contacts_reported(RID p_body, int p_contacts) { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + body->set_max_contacts_reported(p_contacts); +} + +int PhysicsServerSW::body_get_max_contacts_reported(RID p_body) const { + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND_V(!body,-1); + return body->get_max_contacts_reported(); +} + +void PhysicsServerSW::body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata) { + + + BodySW *body = body_owner.get(p_body); + ERR_FAIL_COND(!body); + body->set_force_integration_callback(p_receiver?p_receiver->get_instance_ID():ObjectID(0),p_method,p_udata); + +} + + +/* JOINT API */ + +#if 0 +void PhysicsServerSW::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) { + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND(!joint); + + switch(p_param) { + case JOINT_PARAM_BIAS: joint->set_bias(p_value); break; + case JOINT_PARAM_MAX_BIAS: joint->set_max_bias(p_value); break; + case JOINT_PARAM_MAX_FORCE: joint->set_max_force(p_value); break; + } + + +} + +real_t PhysicsServerSW::joint_get_param(RID p_joint,JointParam p_param) const { + + const JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND_V(!joint,-1); + + switch(p_param) { + case JOINT_PARAM_BIAS: return joint->get_bias(); break; + case JOINT_PARAM_MAX_BIAS: return joint->get_max_bias(); break; + case JOINT_PARAM_MAX_FORCE: return joint->get_max_force(); break; + } + + return 0; +} + + +RID PhysicsServerSW::pin_joint_create(const Vector3& p_pos,RID p_body_a,RID p_body_b) { + + BodySW *A=body_owner.get(p_body_a); + ERR_FAIL_COND_V(!A,RID()); + BodySW *B=NULL; + if (body_owner.owns(p_body_b)) { + B=body_owner.get(p_body_b); + ERR_FAIL_COND_V(!B,RID()); + } + + JointSW *joint = memnew( PinJointSW(p_pos,A,B) ); + RID self = joint_owner.make_rid(joint); + joint->set_self(self); + + return self; +} + +RID PhysicsServerSW::groove_joint_create(const Vector3& p_a_groove1,const Vector3& p_a_groove2, const Vector3& p_b_anchor, RID p_body_a,RID p_body_b) { + + + BodySW *A=body_owner.get(p_body_a); + ERR_FAIL_COND_V(!A,RID()); + + BodySW *B=body_owner.get(p_body_b); + ERR_FAIL_COND_V(!B,RID()); + + JointSW *joint = memnew( GrooveJointSW(p_a_groove1,p_a_groove2,p_b_anchor,A,B) ); + RID self = joint_owner.make_rid(joint); + joint->set_self(self); + return self; + + +} + +RID PhysicsServerSW::damped_spring_joint_create(const Vector3& p_anchor_a,const Vector3& p_anchor_b,RID p_body_a,RID p_body_b) { + + BodySW *A=body_owner.get(p_body_a); + ERR_FAIL_COND_V(!A,RID()); + + BodySW *B=body_owner.get(p_body_b); + ERR_FAIL_COND_V(!B,RID()); + + JointSW *joint = memnew( DampedSpringJointSW(p_anchor_a,p_anchor_b,A,B) ); + RID self = joint_owner.make_rid(joint); + joint->set_self(self); + return self; + +} + +void PhysicsServerSW::damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value) { + + + JointSW *j = joint_owner.get(p_joint); + ERR_FAIL_COND(!j); + ERR_FAIL_COND(j->get_type()!=JOINT_DAMPED_SPRING); + + DampedSpringJointSW *dsj = static_cast<DampedSpringJointSW*>(j); + dsj->set_param(p_param,p_value); +} + +real_t PhysicsServerSW::damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const { + + JointSW *j = joint_owner.get(p_joint); + ERR_FAIL_COND_V(!j,0); + ERR_FAIL_COND_V(j->get_type()!=JOINT_DAMPED_SPRING,0); + + DampedSpringJointSW *dsj = static_cast<DampedSpringJointSW*>(j); + return dsj->get_param(p_param); +} + +PhysicsServer::JointType PhysicsServerSW::joint_get_type(RID p_joint) const { + + + JointSW *joint = joint_owner.get(p_joint); + ERR_FAIL_COND_V(!joint,JOINT_PIN); + + return joint->get_type(); +} + +#endif + +void PhysicsServerSW::free(RID p_rid) { + + if (shape_owner.owns(p_rid)) { + + ShapeSW *shape = shape_owner.get(p_rid); + + while(shape->get_owners().size()) { + ShapeOwnerSW *so=shape->get_owners().front()->key(); + so->remove_shape(shape); + } + + shape_owner.free(p_rid); + memdelete(shape); + } else if (body_owner.owns(p_rid)) { + + BodySW *body = body_owner.get(p_rid); + +// if (body->get_state_query()) +// _clear_query(body->get_state_query()); + +// if (body->get_direct_state_query()) +// _clear_query(body->get_direct_state_query()); + + body->set_space(NULL); + + + while( body->get_shape_count() ) { + + body->remove_shape(0); + } + + while (body->get_constraint_map().size()) { + RID self = body->get_constraint_map().front()->key()->get_self(); + ERR_FAIL_COND(!self.is_valid()); + free(self); + } + + body_owner.free(p_rid); + memdelete(body); + + } else if (area_owner.owns(p_rid)) { + + AreaSW *area = area_owner.get(p_rid); + +// if (area->get_monitor_query()) +// _clear_query(area->get_monitor_query()); + + area->set_space(NULL); + + while( area->get_shape_count() ) { + + area->remove_shape(0); + } + + area_owner.free(p_rid); + memdelete(area); + } else if (space_owner.owns(p_rid)) { + + SpaceSW *space = space_owner.get(p_rid); + + while(space->get_objects().size()) { + CollisionObjectSW *co = (CollisionObjectSW *)space->get_objects().front()->get(); + co->set_space(NULL); + } + + active_spaces.erase(space); + free(space->get_default_area()->get_self()); + space_owner.free(p_rid); + memdelete(space); + } else if (joint_owner.owns(p_rid)) { + + JointSW *joint = joint_owner.get(p_rid); + + joint_owner.free(p_rid); + memdelete(joint); + + } else { + + ERR_EXPLAIN("Invalid ID"); + ERR_FAIL(); + } + + +}; + +void PhysicsServerSW::set_active(bool p_active) { + + active=p_active; +}; + +void PhysicsServerSW::init() { + + doing_sync=true; + last_step=0.001; + iterations=8;// 8? + stepper = memnew( StepSW ); + direct_state = memnew( PhysicsDirectBodyStateSW ); +}; + + +void PhysicsServerSW::step(float p_step) { + + + if (!active) + return; + + + doing_sync=false; + + last_step=p_step; + PhysicsDirectBodyStateSW::singleton->step=p_step; + + for( Set<const SpaceSW*>::Element *E=active_spaces.front();E;E=E->next()) { + + stepper->step((SpaceSW*)E->get(),p_step,iterations); + } +}; + +void PhysicsServerSW::sync() { + +}; + +void PhysicsServerSW::flush_queries() { + + if (!active) + return; + + doing_sync=true; + for( Set<const SpaceSW*>::Element *E=active_spaces.front();E;E=E->next()) { + + SpaceSW *space=(SpaceSW *)E->get(); + space->call_queries(); + } + +}; + + + +void PhysicsServerSW::finish() { + + memdelete(stepper); + memdelete(direct_state); +}; + + +PhysicsServerSW::PhysicsServerSW() { + + BroadPhaseSW::create_func=BroadPhaseOctree::_create; + + active=true; + +}; + +PhysicsServerSW::~PhysicsServerSW() { + +}; + + diff --git a/servers/physics/physics_server_sw.h b/servers/physics/physics_server_sw.h new file mode 100644 index 0000000000..2a46ba65fb --- /dev/null +++ b/servers/physics/physics_server_sw.h @@ -0,0 +1,215 @@ +/*************************************************************************/ +/* physics_server_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef PHYSICS_SERVER_SW +#define PHYSICS_SERVER_SW + + +#include "servers/physics_server.h" +#include "shape_sw.h" +#include "space_sw.h" +#include "step_sw.h" +#include "joints_sw.h" + + +class PhysicsServerSW : public PhysicsServer { + + OBJ_TYPE( PhysicsServerSW, PhysicsServer ); + +friend class PhysicsDirectSpaceStateSW; + bool active; + int iterations; + bool doing_sync; + real_t last_step; + + StepSW *stepper; + Set<const SpaceSW*> active_spaces; + + PhysicsDirectBodyStateSW *direct_state; + + mutable RID_Owner<ShapeSW> shape_owner; + mutable RID_Owner<SpaceSW> space_owner; + mutable RID_Owner<AreaSW> area_owner; + mutable RID_Owner<BodySW> body_owner; + mutable RID_Owner<JointSW> joint_owner; + +// void _clear_query(QuerySW *p_query); +public: + + virtual RID shape_create(ShapeType p_shape); + virtual void shape_set_data(RID p_shape, const Variant& p_data); + virtual void shape_set_custom_solver_bias(RID p_shape, real_t p_bias); + + virtual ShapeType shape_get_type(RID p_shape) const; + virtual Variant shape_get_data(RID p_shape) const; + virtual real_t shape_get_custom_solver_bias(RID p_shape) const; + + /* SPACE API */ + + virtual RID space_create(); + virtual void space_set_active(RID p_space,bool p_active); + virtual bool space_is_active(RID p_space) const; + + virtual void space_set_param(RID p_space,SpaceParameter p_param, real_t p_value); + virtual real_t space_get_param(RID p_space,SpaceParameter p_param) const; + + // this function only works on fixed process, errors and returns null otherwise + virtual PhysicsDirectSpaceState* space_get_direct_state(RID p_space); + + + /* AREA API */ + + virtual RID area_create(); + + virtual void area_set_space_override_mode(RID p_area, AreaSpaceOverrideMode p_mode); + virtual AreaSpaceOverrideMode area_get_space_override_mode(RID p_area) const; + + virtual void area_set_space(RID p_area, RID p_space); + virtual RID area_get_space(RID p_area) const; + + virtual void area_add_shape(RID p_area, RID p_shape, const Transform& p_transform=Transform()); + virtual void area_set_shape(RID p_area, int p_shape_idx,RID p_shape); + virtual void area_set_shape_transform(RID p_area, int p_shape_idx, const Transform& p_transform); + + virtual int area_get_shape_count(RID p_area) const; + virtual RID area_get_shape(RID p_area, int p_shape_idx) const; + virtual Transform area_get_shape_transform(RID p_area, int p_shape_idx) const; + + virtual void area_remove_shape(RID p_area, int p_shape_idx); + virtual void area_clear_shapes(RID p_area); + + virtual void area_attach_object_instance_ID(RID p_area,ObjectID p_ID); + virtual ObjectID area_get_object_instance_ID(RID p_area) const; + + virtual void area_set_param(RID p_area,AreaParameter p_param,const Variant& p_value); + virtual void area_set_transform(RID p_area, const Transform& p_transform); + + virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const; + virtual Transform area_get_transform(RID p_area) const; + + virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method); + + + /* BODY API */ + + // create a body of a given type + virtual RID body_create(BodyMode p_mode=BODY_MODE_RIGID,bool p_init_sleeping=false); + + virtual void body_set_space(RID p_body, RID p_space); + virtual RID body_get_space(RID p_body) const; + + virtual void body_set_mode(RID p_body, BodyMode p_mode); + virtual BodyMode body_get_mode(RID p_body, BodyMode p_mode) const; + + virtual void body_add_shape(RID p_body, RID p_shape, const Transform& p_transform=Transform()); + virtual void body_set_shape(RID p_body, int p_shape_idx,RID p_shape); + virtual void body_set_shape_transform(RID p_body, int p_shape_idx, const Transform& p_transform); + + virtual int body_get_shape_count(RID p_body) const; + virtual RID body_get_shape(RID p_body, int p_shape_idx) const; + virtual Transform body_get_shape_transform(RID p_body, int p_shape_idx) const; + + virtual void body_set_shape_as_trigger(RID p_body, int p_shape_idx,bool p_enable); + virtual bool body_is_shape_set_as_trigger(RID p_body, int p_shape_idx) const; + + virtual void body_remove_shape(RID p_body, int p_shape_idx); + virtual void body_clear_shapes(RID p_body); + + virtual void body_attach_object_instance_ID(RID p_body,uint32_t p_ID); + virtual uint32_t body_get_object_instance_ID(RID p_body) const; + + virtual void body_set_enable_continuous_collision_detection(RID p_body,bool p_enable); + virtual bool body_is_continuous_collision_detection_enabled(RID p_body) const; + + virtual void body_set_user_flags(RID p_body, uint32_t p_flags); + virtual uint32_t body_get_user_flags(RID p_body, uint32_t p_flags) const; + + virtual void body_set_param(RID p_body, BodyParameter p_param, float p_value); + virtual float body_get_param(RID p_body, BodyParameter p_param) const; + + //advanced simulation + virtual void body_static_simulate_motion(RID p_body,const Transform& p_new_transform); + + virtual void body_set_state(RID p_body, BodyState p_state, const Variant& p_variant); + virtual Variant body_get_state(RID p_body, BodyState p_state) const; + + virtual void body_set_applied_force(RID p_body, const Vector3& p_force); + virtual Vector3 body_get_applied_force(RID p_body) const; + + virtual void body_set_applied_torque(RID p_body, const Vector3& p_torque); + virtual Vector3 body_get_applied_torque(RID p_body) const; + + virtual void body_apply_impulse(RID p_body, const Vector3& p_pos, const Vector3& p_impulse); + virtual void body_set_axis_velocity(RID p_body, const Vector3& p_axis_velocity); + + virtual void body_add_collision_exception(RID p_body, RID p_body_b); + virtual void body_remove_collision_exception(RID p_body, RID p_body_b); + virtual void body_get_collision_exceptions(RID p_body, List<RID> *p_exceptions); + + virtual void body_set_contacts_reported_depth_treshold(RID p_body, float p_treshold); + virtual float body_get_contacts_reported_depth_treshold(RID p_body) const; + + virtual void body_set_omit_force_integration(RID p_body,bool p_omit); + virtual bool body_is_omitting_force_integration(RID p_body) const; + + virtual void body_set_max_contacts_reported(RID p_body, int p_contacts); + virtual int body_get_max_contacts_reported(RID p_body) const; + + virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant()); + + /* JOINT API */ +#if 0 + virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value); + virtual real_t joint_get_param(RID p_joint,JointParam p_param) const; + + virtual RID pin_joint_create(const Vector3& p_pos,RID p_body_a,RID p_body_b=RID()); + virtual RID groove_joint_create(const Vector3& p_a_groove1,const Vector3& p_a_groove2, const Vector3& p_b_anchor, RID p_body_a,RID p_body_b); + virtual RID damped_spring_joint_create(const Vector3& p_anchor_a,const Vector3& p_anchor_b,RID p_body_a,RID p_body_b=RID()); + virtual void damped_string_joint_set_param(RID p_joint, DampedStringParam p_param, real_t p_value); + virtual real_t damped_string_joint_get_param(RID p_joint, DampedStringParam p_param) const; + + virtual JointType joint_get_type(RID p_joint) const; +#endif + /* MISC */ + + virtual void free(RID p_rid); + + virtual void set_active(bool p_active); + virtual void init(); + virtual void step(float p_step); + virtual void sync(); + virtual void flush_queries(); + virtual void finish(); + + PhysicsServerSW(); + ~PhysicsServerSW(); + +}; + +#endif + diff --git a/servers/physics/shape_sw.cpp b/servers/physics/shape_sw.cpp new file mode 100644 index 0000000000..70e5c00b92 --- /dev/null +++ b/servers/physics/shape_sw.cpp @@ -0,0 +1,1664 @@ +/*************************************************************************/ +/* shape_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "shape_sw.h"
+#include "geometry.h"
+#include "sort.h"
+#include "quick_hull.h"
+#define _POINT_SNAP 0.001953125
+#define _EDGE_IS_VALID_SUPPORT_TRESHOLD 0.0002
+#define _FACE_IS_VALID_SUPPORT_TRESHOLD 0.9998
+
+
+void ShapeSW::configure(const AABB& p_aabb) {
+ aabb=p_aabb;
+ configured=true;
+ for (Map<ShapeOwnerSW*,int>::Element *E=owners.front();E;E=E->next()) {
+ ShapeOwnerSW* co=(ShapeOwnerSW*)E->key();
+ co->_shape_changed();
+ }
+}
+
+
+Vector3 ShapeSW::get_support(const Vector3& p_normal) const {
+
+ Vector3 res;
+ int amnt;
+ get_supports(p_normal,1,&res,amnt);
+ return res;
+}
+
+void ShapeSW::add_owner(ShapeOwnerSW *p_owner) {
+
+ Map<ShapeOwnerSW*,int>::Element *E=owners.find(p_owner);
+ if (E) {
+ E->get()++;
+ } else {
+ owners[p_owner]=1;
+ }
+}
+
+void ShapeSW::remove_owner(ShapeOwnerSW *p_owner){
+
+ Map<ShapeOwnerSW*,int>::Element *E=owners.find(p_owner);
+ ERR_FAIL_COND(!E);
+ E->get()--;
+ if (E->get()==0) {
+ owners.erase(E);
+ }
+
+}
+
+bool ShapeSW::is_owner(ShapeOwnerSW *p_owner) const{
+
+ return owners.has(p_owner);
+
+}
+
+const Map<ShapeOwnerSW*,int>& ShapeSW::get_owners() const{
+ return owners;
+}
+
+
+ShapeSW::ShapeSW() {
+
+ custom_bias=0;
+ configured=false;
+}
+
+
+ShapeSW::~ShapeSW() {
+
+ ERR_FAIL_COND(owners.size());
+}
+
+
+
+Plane PlaneShapeSW::get_plane() const {
+
+ return plane;
+}
+
+void PlaneShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+ // gibberish, a plane is infinity
+ r_min=-1e7;
+ r_max=1e7;
+}
+
+Vector3 PlaneShapeSW::get_support(const Vector3& p_normal) const {
+
+ return p_normal*1e15;
+}
+
+
+bool PlaneShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+
+ bool inters=plane.intersects_segment(p_begin,p_end,&r_result);
+ if(inters)
+ r_normal=plane.normal;
+ return inters;
+}
+
+Vector3 PlaneShapeSW::get_moment_of_inertia(float p_mass) const {
+
+ return Vector3(); //wtf
+}
+
+void PlaneShapeSW::_setup(const Plane& p_plane) {
+
+ plane=p_plane;
+ configure(AABB(Vector3(-1e4,-1e4,-1e4),Vector3(1e4*2,1e4*2,1e4*2)));
+}
+
+void PlaneShapeSW::set_data(const Variant& p_data) {
+
+ _setup(p_data);
+
+}
+
+Variant PlaneShapeSW::get_data() const {
+
+ return plane;
+}
+
+PlaneShapeSW::PlaneShapeSW() {
+
+
+}
+
+//
+
+float RayShapeSW::get_length() const {
+
+ return length;
+}
+
+void RayShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+ // don't think this will be even used
+ r_min=0;
+ r_max=1;
+}
+
+Vector3 RayShapeSW::get_support(const Vector3& p_normal) const {
+
+ if (p_normal.z>0)
+ return Vector3(0,0,length);
+ else
+ return Vector3(0,0,0);
+}
+
+void RayShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
+
+ if (Math::abs(p_normal.z) < _EDGE_IS_VALID_SUPPORT_TRESHOLD) {
+
+ r_amount=2;
+ r_supports[0]=Vector3(0,0,0);
+ r_supports[1]=Vector3(0,0,length);
+ } if (p_normal.z>0) {
+ r_amount=1;
+ *r_supports=Vector3(0,0,length);
+ } else {
+ r_amount=1;
+ *r_supports=Vector3(0,0,0);
+ }
+}
+
+
+bool RayShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+
+ return false; //simply not possible
+}
+
+Vector3 RayShapeSW::get_moment_of_inertia(float p_mass) const {
+
+ return Vector3();
+}
+
+void RayShapeSW::_setup(float p_length) {
+
+ length=p_length;
+ configure(AABB(Vector3(0,0,0),Vector3(0.1,0.1,length)));
+}
+
+void RayShapeSW::set_data(const Variant& p_data) {
+
+ _setup(p_data);
+
+}
+
+Variant RayShapeSW::get_data() const {
+
+ return length;
+}
+
+RayShapeSW::RayShapeSW() {
+
+ length=1;
+}
+
+
+
+/********** SPHERE *************/
+
+real_t SphereShapeSW::get_radius() const {
+
+ return radius;
+}
+
+void SphereShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+ float d = p_normal.dot( p_transform.origin );
+
+ // figure out scale at point
+ Vector3 local_normal = p_transform.basis.xform_inv(p_normal);
+ float scale = local_normal.length();
+
+ r_min = d - (radius) * scale;
+ r_max = d + (radius) * scale;
+
+}
+
+Vector3 SphereShapeSW::get_support(const Vector3& p_normal) const {
+
+ return p_normal*radius;
+}
+
+void SphereShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
+
+ *r_supports=p_normal*radius;
+ r_amount=1;
+}
+
+bool SphereShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+
+ return Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(),radius,&r_result,&r_normal);
+}
+
+Vector3 SphereShapeSW::get_moment_of_inertia(float p_mass) const {
+
+ float s = 0.4 * p_mass * radius * radius;
+ return Vector3(s,s,s);
+}
+
+void SphereShapeSW::_setup(real_t p_radius) {
+
+
+ radius=p_radius;
+ configure(AABB( Vector3(-radius,-radius,-radius), Vector3(radius*2.0,radius*2.0,radius*2.0)));
+
+}
+
+void SphereShapeSW::set_data(const Variant& p_data) {
+
+ _setup(p_data);
+}
+
+Variant SphereShapeSW::get_data() const {
+
+ return radius;
+}
+
+SphereShapeSW::SphereShapeSW() {
+
+ radius=0;
+}
+
+
+/********** BOX *************/
+
+
+void BoxShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+ // no matter the angle, the box is mirrored anyway
+ Vector3 local_normal=p_transform.basis.xform_inv(p_normal);
+
+ float length = local_normal.abs().dot(half_extents);
+ float distance = p_normal.dot( p_transform.origin );
+
+ r_min = distance - length;
+ r_max = distance + length;
+
+
+}
+
+Vector3 BoxShapeSW::get_support(const Vector3& p_normal) const {
+
+
+ Vector3 point(
+ (p_normal.x<0) ? -half_extents.x : half_extents.x,
+ (p_normal.y<0) ? -half_extents.y : half_extents.y,
+ (p_normal.z<0) ? -half_extents.z : half_extents.z
+ );
+
+ return point;
+}
+
+void BoxShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
+
+ static const int next[3]={1,2,0};
+ static const int next2[3]={2,0,1};
+
+ for (int i=0;i<3;i++) {
+
+ Vector3 axis;
+ axis[i]=1.0;
+ float dot = p_normal.dot( axis );
+ if ( Math::abs( dot ) > _FACE_IS_VALID_SUPPORT_TRESHOLD ) {
+
+ //Vector3 axis_b;
+
+ bool neg = dot<0;
+ r_amount = 4;
+
+ Vector3 point;
+ point[i]=half_extents[i];
+
+ int i_n=next[i];
+ int i_n2=next2[i];
+
+ static const float sign[4][2]={
+
+ {-1.0, 1.0},
+ { 1.0, 1.0},
+ { 1.0,-1.0},
+ {-1.0,-1.0},
+ };
+
+ for (int j=0;j<4;j++) {
+
+ point[i_n]=sign[j][0]*half_extents[i_n];
+ point[i_n2]=sign[j][1]*half_extents[i_n2];
+ r_supports[j]=neg?-point:point;
+
+ }
+
+ if (neg) {
+ SWAP( r_supports[1], r_supports[2] );
+ SWAP( r_supports[0], r_supports[3] );
+ }
+
+ return;
+ }
+
+ r_amount=0;
+
+ }
+
+ for (int i=0;i<3;i++) {
+
+ Vector3 axis;
+ axis[i]=1.0;
+
+ if (Math::abs(p_normal.dot(axis))<_EDGE_IS_VALID_SUPPORT_TRESHOLD) {
+
+ r_amount= 2;
+
+ int i_n=next[i];
+ int i_n2=next2[i];
+
+ Vector3 point=half_extents;
+
+ if (p_normal[i_n]<0) {
+ point[i_n]=-point[i_n];
+ }
+ if (p_normal[i_n2]<0) {
+ point[i_n2]=-point[i_n2];
+ }
+
+ r_supports[0] = point;
+ point[i]=-point[i];
+ r_supports[1] = point;
+ return;
+ }
+ }
+ /* USE POINT */
+
+ Vector3 point(
+ (p_normal.x<0) ? -half_extents.x : half_extents.x,
+ (p_normal.y<0) ? -half_extents.y : half_extents.y,
+ (p_normal.z<0) ? -half_extents.z : half_extents.z
+ );
+
+ r_amount=1;
+ r_supports[0]=point;
+}
+
+bool BoxShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+
+ AABB aabb(-half_extents,half_extents*2.0);
+
+ return aabb.intersects_segment(p_begin,p_end,&r_result,&r_normal);
+
+}
+
+Vector3 BoxShapeSW::get_moment_of_inertia(float p_mass) const {
+
+ float lx=half_extents.x;
+ float ly=half_extents.y;
+ float lz=half_extents.z;
+
+ return Vector3( (p_mass/3.0) * (ly*ly + lz*lz), (p_mass/3.0) * (lx*lx + lz*lz), (p_mass/3.0) * (lx*lx + ly*ly) );
+
+}
+
+void BoxShapeSW::_setup(const Vector3& p_half_extents) {
+
+ half_extents=p_half_extents.abs();
+
+ configure(AABB(-half_extents,half_extents*2));
+
+
+}
+
+void BoxShapeSW::set_data(const Variant& p_data) {
+
+
+ _setup(p_data);
+}
+
+Variant BoxShapeSW::get_data() const {
+
+ return half_extents;
+}
+
+BoxShapeSW::BoxShapeSW() {
+
+
+}
+
+
+/********** CAPSULE *************/
+
+
+void CapsuleShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+ Vector3 n=p_transform.basis.xform_inv(p_normal).normalized();
+ float h = (n.z > 0) ? height : -height;
+
+ n *= radius;
+ n.z += h * 0.5;
+
+ r_max=p_normal.dot(p_transform.xform(n));
+ r_min=p_normal.dot(p_transform.xform(-n));
+ return;
+
+ n = p_transform.basis.xform(n);
+
+ float distance = p_normal.dot( p_transform.origin );
+ float length = Math::abs(p_normal.dot(n));
+ r_min = distance - length;
+ r_max = distance + length;
+
+ ERR_FAIL_COND( r_max < r_min );
+
+}
+
+Vector3 CapsuleShapeSW::get_support(const Vector3& p_normal) const {
+
+ Vector3 n=p_normal;
+
+ float h = (n.z > 0) ? height : -height;
+
+ n*=radius;
+ n.z += h*0.5;
+ return n;
+}
+
+void CapsuleShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
+
+
+ Vector3 n=p_normal;
+
+ float d = n.z;
+
+ if (Math::abs( d )<_EDGE_IS_VALID_SUPPORT_TRESHOLD ) {
+
+ // make it flat
+ n.z=0.0;
+ n.normalize();
+ n*=radius;
+
+ r_amount=2;
+ r_supports[0]=n;
+ r_supports[0].z+=height*0.5;
+ r_supports[1]=n;
+ r_supports[1].z-=height*0.5;
+
+ } else {
+
+ float h = (d > 0) ? height : -height;
+
+ n*=radius;
+ n.z += h*0.5;
+ r_amount=1;
+ *r_supports=n;
+
+ }
+
+}
+
+
+bool CapsuleShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+
+ Vector3 norm=(p_end-p_begin).normalized();
+ float min_d=1e20;
+
+
+ Vector3 res,n;
+ bool collision=false;
+
+ Vector3 auxres,auxn;
+ bool collided;
+
+ // test against cylinder and spheres :-|
+
+ collided = Geometry::segment_intersects_cylinder(p_begin,p_end,height,radius,&auxres,&auxn);
+
+ if (collided) {
+ float d=norm.dot(auxres);
+ if (d<min_d) {
+ min_d=d;
+ res=auxres;
+ n=auxn;
+ collision=true;
+ }
+ }
+
+ collided = Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(0,0,height*0.5),radius,&auxres,&auxn);
+
+ if (collided) {
+ float d=norm.dot(auxres);
+ if (d<min_d) {
+ min_d=d;
+ res=auxres;
+ n=auxn;
+ collision=true;
+ }
+ }
+
+ collided = Geometry::segment_intersects_sphere(p_begin,p_end,Vector3(0,0,height*-0.5),radius,&auxres,&auxn);
+
+ if (collided) {
+ float d=norm.dot(auxres);
+
+ if (d<min_d) {
+ min_d=d;
+ res=auxres;
+ n=auxn;
+ collision=true;
+ }
+ }
+
+ if (collision) {
+
+ r_result=res;
+ r_normal=n;
+ }
+ return collision;
+}
+
+Vector3 CapsuleShapeSW::get_moment_of_inertia(float p_mass) const {
+
+ // use crappy AABB approximation
+ Vector3 extents=get_aabb().size*0.5;
+
+ return Vector3(
+ (p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),
+ (p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),
+ (p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)
+ );
+
+}
+
+
+
+
+void CapsuleShapeSW::_setup(real_t p_height,real_t p_radius) {
+
+ height=p_height;
+ radius=p_radius;
+ configure(AABB(Vector3(-radius,-radius,-height*0.5-radius),Vector3(radius*2,radius*2,height+radius*2.0)));
+
+}
+
+void CapsuleShapeSW::set_data(const Variant& p_data) {
+
+ Dictionary d = p_data;
+ ERR_FAIL_COND(!d.has("radius"));
+ ERR_FAIL_COND(!d.has("height"));
+ _setup(d["height"],d["radius"]);
+
+}
+
+Variant CapsuleShapeSW::get_data() const {
+
+ Dictionary d;
+ d["radius"]=radius;
+ d["height"]=height;
+ return d;
+
+}
+
+
+CapsuleShapeSW::CapsuleShapeSW() {
+
+ height=radius=0;
+
+}
+
+/********** CONVEX POLYGON *************/
+
+
+void ConvexPolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+
+ int vertex_count=mesh.vertices.size();
+ if (vertex_count==0)
+ return;
+
+ const Vector3 *vrts=&mesh.vertices[0];
+
+ for (int i=0;i<vertex_count;i++) {
+
+ float d=p_normal.dot( p_transform.xform( vrts[i] ) );
+
+ if (i==0 || d > r_max)
+ r_max=d;
+ if (i==0 || d < r_min)
+ r_min=d;
+ }
+}
+
+Vector3 ConvexPolygonShapeSW::get_support(const Vector3& p_normal) const {
+
+ Vector3 n=p_normal;
+
+ int vert_support_idx=-1;
+ float support_max;
+
+ int vertex_count=mesh.vertices.size();
+ if (vertex_count==0)
+ return Vector3();
+
+ const Vector3 *vrts=&mesh.vertices[0];
+
+ for (int i=0;i<vertex_count;i++) {
+
+ float d=n.dot(vrts[i]);
+
+ if (i==0 || d > support_max) {
+ support_max=d;
+ vert_support_idx=i;
+ }
+ }
+
+ return vrts[vert_support_idx];
+
+}
+
+
+
+void ConvexPolygonShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
+
+ const Geometry::MeshData::Face *faces = mesh.faces.ptr();
+ int fc = mesh.faces.size();
+
+ const Geometry::MeshData::Edge *edges = mesh.edges.ptr();
+ int ec = mesh.edges.size();
+
+ const Vector3 *vertices = mesh.vertices.ptr();
+ int vc = mesh.vertices.size();
+
+ //find vertex first
+ real_t max;
+ int vtx;
+
+ for (int i=0;i<vc;i++) {
+
+ float d=p_normal.dot(vertices[i]);
+
+ if (i==0 || d > max) {
+ max=d;
+ vtx=i;
+ }
+ }
+
+
+ for(int i=0;i<fc;i++) {
+
+ if (faces[i].plane.normal.dot(p_normal)>_FACE_IS_VALID_SUPPORT_TRESHOLD) {
+
+ int ic = faces[i].indices.size();
+ const int *ind=faces[i].indices.ptr();
+
+ bool valid=false;
+ for(int j=0;j<ic;j++) {
+ if (ind[j]==vtx) {
+ valid=true;
+ break;
+ }
+ }
+
+ if (!valid)
+ continue;
+
+ int m = MIN(p_max,ic);
+ for(int j=0;j<m;j++) {
+
+ r_supports[j]=vertices[ind[j]];
+ }
+ r_amount=m;
+ return;
+ }
+ }
+
+ for(int i=0;i<ec;i++) {
+
+
+ float dot=(vertices[edges[i].a]-vertices[edges[i].b]).normalized().dot(p_normal);
+ dot=ABS(dot);
+ if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD && (edges[i].a==vtx || edges[i].b==vtx)) {
+
+ r_amount=2;
+ r_supports[0]=vertices[edges[i].a];
+ r_supports[1]=vertices[edges[i].b];
+ return;
+ }
+ }
+
+
+ r_supports[0]=vertices[vtx];
+ r_amount=1;
+}
+
+bool ConvexPolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+
+
+
+ const Geometry::MeshData::Face *faces = mesh.faces.ptr();
+ int fc = mesh.faces.size();
+
+ const Vector3 *vertices = mesh.vertices.ptr();
+ int vc = mesh.vertices.size();
+
+ Vector3 n = p_end-p_begin;
+ float min = 1e20;
+ bool col=false;
+
+ for(int i=0;i<fc;i++) {
+
+ if (faces[i].plane.normal.dot(n) > 0)
+ continue; //opposing face
+
+ int ic = faces[i].indices.size();
+ const int *ind=faces[i].indices.ptr();
+
+ for(int j=1;j<ic-1;j++) {
+
+ Face3 f(vertices[ind[0]],vertices[ind[i]],vertices[ind[i+1]]);
+ Vector3 result;
+ if (f.intersects_segment(p_begin,p_end,&result)) {
+ float d = n.dot(result);
+ if (d<min) {
+ min=d;
+ r_result=result;
+ r_normal=faces[i].plane.normal;
+ col=true;
+ }
+
+ break;
+ }
+
+ }
+ }
+
+ return col;
+
+}
+
+Vector3 ConvexPolygonShapeSW::get_moment_of_inertia(float p_mass) const {
+
+ // use crappy AABB approximation
+ Vector3 extents=get_aabb().size*0.5;
+
+ return Vector3(
+ (p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),
+ (p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),
+ (p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)
+ );
+
+}
+
+void ConvexPolygonShapeSW::_setup(const Vector<Vector3>& p_vertices) {
+
+ Error err = QuickHull::build(p_vertices,mesh);
+ AABB _aabb;
+
+ for(int i=0;i<mesh.vertices.size();i++) {
+
+ if (i==0)
+ _aabb.pos=mesh.vertices[i];
+ else
+ _aabb.expand_to(mesh.vertices[i]);
+ }
+
+ configure(_aabb);
+
+
+}
+
+void ConvexPolygonShapeSW::set_data(const Variant& p_data) {
+
+ _setup(p_data);
+
+}
+
+Variant ConvexPolygonShapeSW::get_data() const {
+
+ return mesh.vertices;
+}
+
+
+ConvexPolygonShapeSW::ConvexPolygonShapeSW() {
+
+
+}
+
+
+/********** FACE POLYGON *************/
+
+
+void FaceShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+ for (int i=0;i<3;i++) {
+
+ Vector3 v=p_transform.xform(vertex[i]);
+ float d=p_normal.dot(v);
+
+ if (i==0 || d > r_max)
+ r_max=d;
+
+ if (i==0 || d < r_min)
+ r_min=d;
+ }
+}
+
+Vector3 FaceShapeSW::get_support(const Vector3& p_normal) const {
+
+
+ Vector3 n=p_normal;
+
+ int vert_support_idx=-1;
+ float support_max;
+
+ for (int i=0;i<3;i++) {
+
+ //float d=n.dot(vertex[i]);
+ float d=p_normal.dot(vertex[i]);
+
+ if (i==0 || d > support_max) {
+ support_max=d;
+ vert_support_idx=i;
+ }
+ }
+
+ return vertex[vert_support_idx];
+}
+
+void FaceShapeSW::get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const {
+
+ Vector3 n=p_normal;
+
+ /** TEST FACE AS SUPPORT **/
+ if (normal.dot(n) > _FACE_IS_VALID_SUPPORT_TRESHOLD) {
+
+ r_amount=3;
+ for (int i=0;i<3;i++) {
+
+ r_supports[i]=vertex[i];
+ }
+ return;
+
+ }
+
+ /** FIND SUPPORT VERTEX **/
+
+ int vert_support_idx=-1;
+ float support_max;
+
+ for (int i=0;i<3;i++) {
+
+ float d=n.dot(vertex[i]);
+
+ if (i==0 || d > support_max) {
+ support_max=d;
+ vert_support_idx=i;
+ }
+ }
+
+ /** TEST EDGES AS SUPPORT **/
+
+ for (int i=0;i<3;i++) {
+
+ int nx=(i+1)%3;
+ //if (i!=vert_support_idx && nx!=vert_support_idx)
+ // continue;
+
+ // check if edge is valid as a support
+ float dot=(vertex[i]-vertex[nx]).normalized().dot(n);
+ dot=ABS(dot);
+ if (dot < _EDGE_IS_VALID_SUPPORT_TRESHOLD) {
+
+ r_amount=2;
+ r_supports[0]=vertex[i];
+ r_supports[1]=vertex[nx];
+ return;
+ }
+ }
+
+ r_amount=1;
+ r_supports[0]=vertex[vert_support_idx];
+}
+
+bool FaceShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+
+
+ bool c=Geometry::segment_intersects_triangle(p_begin,p_end,vertex[0],vertex[1],vertex[2],&r_result);
+ if (c)
+ r_normal=Plane(vertex[0],vertex[1],vertex[2]).normal;
+
+ return c;
+}
+
+Vector3 FaceShapeSW::get_moment_of_inertia(float p_mass) const {
+
+ return Vector3(); // Sorry, but i don't think anyone cares, FaceShape!
+
+}
+
+FaceShapeSW::FaceShapeSW() {
+
+ configure(AABB());
+
+}
+
+
+
+DVector<Vector3> ConcavePolygonShapeSW::get_faces() const {
+
+
+ DVector<Vector3> rfaces;
+ rfaces.resize(faces.size()*3);
+
+ for(int i=0;i<faces.size();i++) {
+
+ Face f=faces.get(i);
+
+ for(int j=0;j<3;j++) {
+
+ rfaces.set(i*3+j, vertices.get( f.indices[j] ) );
+ }
+ }
+
+ return rfaces;
+}
+
+void ConcavePolygonShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+ int count=vertices.size();
+ DVector<Vector3>::Read r=vertices.read();
+ const Vector3 *vptr=r.ptr();
+
+ for (int i=0;i<count;i++) {
+
+ float d=p_normal.dot( p_transform.xform( vptr[i] ) );
+
+ if (i==0 || d > r_max)
+ r_max=d;
+ if (i==0 || d < r_min)
+ r_min=d;
+
+ }
+}
+
+Vector3 ConcavePolygonShapeSW::get_support(const Vector3& p_normal) const {
+
+
+ int count=vertices.size();
+ DVector<Vector3>::Read r=vertices.read();
+ const Vector3 *vptr=r.ptr();
+
+ Vector3 n=p_normal;
+
+ int vert_support_idx=-1;
+ float support_max;
+
+ for (int i=0;i<count;i++) {
+
+ float d=n.dot(vptr[i]);
+
+ if (i==0 || d > support_max) {
+ support_max=d;
+ vert_support_idx=i;
+ }
+ }
+
+
+ return vptr[vert_support_idx];
+
+}
+
+void ConcavePolygonShapeSW::_cull_segment(int p_idx,_SegmentCullParams *p_params) const {
+
+ const BVH *bvh=&p_params->bvh[p_idx];
+
+
+ //if (p_params->dir.dot(bvh->aabb.get_support(-p_params->dir))>p_params->min_d)
+ // return; //test against whole AABB, which isn't very costly
+
+
+ //printf("addr: %p\n",bvh);
+ if (!bvh->aabb.intersects_segment(p_params->from,p_params->to)) {
+
+ return;
+ }
+
+
+ if (bvh->face_index>=0) {
+
+
+ Vector3 res;
+ Vector3 vertices[3]={
+ p_params->vertices[ p_params->faces[ bvh->face_index ].indices[0] ],
+ p_params->vertices[ p_params->faces[ bvh->face_index ].indices[1] ],
+ p_params->vertices[ p_params->faces[ bvh->face_index ].indices[2] ]
+ };
+
+ if (Geometry::segment_intersects_triangle(
+ p_params->from,
+ p_params->to,
+ vertices[0],
+ vertices[1],
+ vertices[2],
+ &res)) {
+
+
+ float d=p_params->normal.dot(res) - p_params->normal.dot(p_params->from);
+ //TODO, seems segmen/triangle intersection is broken :(
+ if (d>0 && d<p_params->min_d) {
+
+ p_params->min_d=d;
+ p_params->result=res;
+ p_params->normal=Plane(vertices[0],vertices[1],vertices[2]).normal;
+ p_params->collisions++;
+ }
+
+ }
+
+
+
+ } else {
+
+ if (bvh->left>=0)
+ _cull_segment(bvh->left,p_params);
+ if (bvh->right>=0)
+ _cull_segment(bvh->right,p_params);
+
+
+ }
+}
+
+bool ConcavePolygonShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const {
+
+ // unlock data
+ DVector<Face>::Read fr=faces.read();
+ DVector<Vector3>::Read vr=vertices.read();
+ DVector<BVH>::Read br=bvh.read();
+
+
+ _SegmentCullParams params;
+ params.from=p_begin;
+ params.to=p_end;
+ params.collisions=0;
+ params.normal=(p_end-p_begin).normalized();
+
+ params.faces=fr.ptr();
+ params.vertices=vr.ptr();
+ params.bvh=br.ptr();
+
+ params.min_d=1e20;
+ // cull
+ _cull_segment(0,¶ms);
+
+ if (params.collisions>0) {
+
+
+ r_result=params.result;
+ r_normal=params.normal;
+ return true;
+ } else {
+
+ return false;
+ }
+}
+
+void ConcavePolygonShapeSW::_cull(int p_idx,_CullParams *p_params) const {
+
+ const BVH* bvh=&p_params->bvh[p_idx];
+
+ if (!p_params->aabb.intersects( bvh->aabb ))
+ return;
+
+ if (bvh->face_index>=0) {
+
+ const Face *f=&p_params->faces[ bvh->face_index ];
+ FaceShapeSW *face=p_params->face;
+ face->normal=f->normal;
+ face->vertex[0]=p_params->vertices[f->indices[0]];
+ face->vertex[1]=p_params->vertices[f->indices[1]];
+ face->vertex[2]=p_params->vertices[f->indices[2]];
+ p_params->callback(p_params->userdata,face);
+
+ } else {
+
+ if (bvh->left>=0) {
+
+ _cull(bvh->left,p_params);
+
+ }
+
+ if (bvh->right>=0) {
+
+ _cull(bvh->right,p_params);
+ }
+
+ }
+}
+
+void ConcavePolygonShapeSW::cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const {
+
+ // make matrix local to concave
+
+ AABB local_aabb=p_local_aabb;
+
+ // unlock data
+ DVector<Face>::Read fr=faces.read();
+ DVector<Vector3>::Read vr=vertices.read();
+ DVector<BVH>::Read br=bvh.read();
+
+ FaceShapeSW face; // use this to send in the callback
+
+ _CullParams params;
+ params.aabb=local_aabb;
+ params.face=&face;
+ params.faces=fr.ptr();
+ params.vertices=vr.ptr();
+ params.bvh=br.ptr();
+ params.callback=p_callback;
+ params.userdata=p_userdata;
+
+ // cull
+ _cull(0,¶ms);
+
+}
+
+Vector3 ConcavePolygonShapeSW::get_moment_of_inertia(float p_mass) const {
+
+ // use crappy AABB approximation
+ Vector3 extents=get_aabb().size*0.5;
+
+ return Vector3(
+ (p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),
+ (p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),
+ (p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)
+ );
+}
+
+
+struct _VolumeSW_BVH_Element {
+
+ AABB aabb;
+ Vector3 center;
+ int face_index;
+};
+
+struct _VolumeSW_BVH_CompareX {
+
+ _FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const {
+
+ return a.center.x<b.center.x;
+ }
+};
+
+
+struct _VolumeSW_BVH_CompareY {
+
+ _FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const {
+
+ return a.center.y<b.center.y;
+ }
+};
+
+struct _VolumeSW_BVH_CompareZ {
+
+ _FORCE_INLINE_ bool operator ()(const _VolumeSW_BVH_Element& a, const _VolumeSW_BVH_Element& b) const {
+
+ return a.center.z<b.center.z;
+ }
+};
+
+struct _VolumeSW_BVH {
+
+ AABB aabb;
+ _VolumeSW_BVH *left;
+ _VolumeSW_BVH *right;
+
+ int face_index;
+};
+
+
+_VolumeSW_BVH* _volume_sw_build_bvh(_VolumeSW_BVH_Element *p_elements,int p_size,int &count) {
+
+ _VolumeSW_BVH* bvh = memnew( _VolumeSW_BVH );
+
+ if (p_size==1) {
+ //leaf
+ bvh->aabb=p_elements[0].aabb;
+ bvh->left=NULL;
+ bvh->right=NULL;
+ bvh->face_index=p_elements->face_index;
+ count++;
+ return bvh;
+ } else {
+
+ bvh->face_index=-1;
+ }
+
+ AABB aabb;
+ for(int i=0;i<p_size;i++) {
+
+ if (i==0)
+ aabb=p_elements[i].aabb;
+ else
+ aabb.merge_with(p_elements[i].aabb);
+ }
+ bvh->aabb=aabb;
+ switch(aabb.get_longest_axis_index()) {
+
+ case 0: {
+
+ SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareX> sort_x;
+ sort_x.sort(p_elements,p_size);
+
+ } break;
+ case 1: {
+
+ SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareY> sort_y;
+ sort_y.sort(p_elements,p_size);
+ } break;
+ case 2: {
+
+ SortArray<_VolumeSW_BVH_Element,_VolumeSW_BVH_CompareZ> sort_z;
+ sort_z.sort(p_elements,p_size);
+ } break;
+ }
+
+ int split=p_size/2;
+ bvh->left=_volume_sw_build_bvh(p_elements,split,count);
+ bvh->right=_volume_sw_build_bvh(&p_elements[split],p_size-split,count);
+
+// printf("branch at %p - %i: %i\n",bvh,count,bvh->face_index);
+ count++;
+ return bvh;
+}
+
+
+void ConcavePolygonShapeSW::_fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx) {
+
+ int idx=p_idx;
+
+
+ p_bvh_array[idx].aabb=p_bvh_tree->aabb;
+ p_bvh_array[idx].face_index=p_bvh_tree->face_index;
+// printf("%p - %i: %i(%p) -- %p:%p\n",%p_bvh_array[idx],p_idx,p_bvh_array[i]->face_index,&p_bvh_tree->face_index,p_bvh_tree->left,p_bvh_tree->right);
+
+
+ if (p_bvh_tree->left) {
+ p_bvh_array[idx].left=++p_idx;
+ _fill_bvh(p_bvh_tree->left,p_bvh_array,p_idx);
+
+ } else {
+
+ p_bvh_array[p_idx].left=-1;
+ }
+
+ if (p_bvh_tree->right) {
+ p_bvh_array[idx].right=++p_idx;
+ _fill_bvh(p_bvh_tree->right,p_bvh_array,p_idx);
+
+ } else {
+
+ p_bvh_array[p_idx].right=-1;
+ }
+
+ memdelete(p_bvh_tree);
+
+}
+
+void ConcavePolygonShapeSW::_setup(DVector<Vector3> p_faces) {
+
+ int src_face_count=p_faces.size();
+ ERR_FAIL_COND(src_face_count%3);
+ src_face_count/=3;
+
+ DVector<Vector3>::Read r = p_faces.read();
+ const Vector3 * facesr= r.ptr();
+
+#if 0
+ Map<Vector3,int> point_map;
+ List<Face> face_list;
+
+
+ for(int i=0;i<src_face_count;i++) {
+
+ Face3 faceaux;
+
+ for(int j=0;j<3;j++) {
+
+ faceaux.vertex[j]=facesr[i*3+j].snapped(_POINT_SNAP);
+ //faceaux.vertex[j]=facesr[i*3+j];//facesr[i*3+j].snapped(_POINT_SNAP);
+ }
+
+ ERR_CONTINUE( faceaux.is_degenerate() );
+
+ Face face;
+
+ for(int j=0;j<3;j++) {
+
+
+ Map<Vector3,int>::Element *E=point_map.find(faceaux.vertex[j]);
+ if (E) {
+
+ face.indices[j]=E->value();
+ } else {
+
+ face.indices[j]=point_map.size();
+ point_map.insert(faceaux.vertex[j],point_map.size());
+
+ }
+ }
+
+ face_list.push_back(face);
+ }
+
+ vertices.resize( point_map.size() );
+
+ DVector<Vector3>::Write vw = vertices.write();
+ Vector3 *verticesw=vw.ptr();
+
+ AABB _aabb;
+
+ for( Map<Vector3,int>::Element *E=point_map.front();E;E=E->next()) {
+
+ if (E==point_map.front()) {
+ _aabb.pos=E->key();
+ } else {
+
+ _aabb.expand_to(E->key());
+ }
+ verticesw[E->value()]=E->key();
+ }
+
+ point_map.clear(); // not needed anymore
+
+ faces.resize(face_list.size());
+ DVector<Face>::Write w = faces.write();
+ Face *facesw=w.ptr();
+
+ int fc=0;
+
+ for( List<Face>::Element *E=face_list.front();E;E=E->next()) {
+
+ facesw[fc++]=E->get();
+ }
+
+ face_list.clear();
+
+
+ DVector<_VolumeSW_BVH_Element> bvh_array;
+ bvh_array.resize( fc );
+
+ DVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write();
+ _VolumeSW_BVH_Element *bvh_arrayw=bvhw.ptr();
+
+
+ for(int i=0;i<fc;i++) {
+
+ AABB face_aabb;
+ face_aabb.pos=verticesw[facesw[i].indices[0]];
+ face_aabb.expand_to( verticesw[facesw[i].indices[1]] );
+ face_aabb.expand_to( verticesw[facesw[i].indices[2]] );
+
+ bvh_arrayw[i].face_index=i;
+ bvh_arrayw[i].aabb=face_aabb;
+ bvh_arrayw[i].center=face_aabb.pos+face_aabb.size*0.5;
+
+ }
+
+ w=DVector<Face>::Write();
+ vw=DVector<Vector3>::Write();
+
+
+ int count=0;
+ _VolumeSW_BVH *bvh_tree=_volume_sw_build_bvh(bvh_arrayw,fc,count);
+
+ ERR_FAIL_COND(count==0);
+
+ bvhw=DVector<_VolumeSW_BVH_Element>::Write();
+
+ bvh.resize( count+1 );
+
+ DVector<BVH>::Write bvhw2 = bvh.write();
+ BVH*bvh_arrayw2=bvhw2.ptr();
+
+ int idx=0;
+ _fill_bvh(bvh_tree,bvh_arrayw2,idx);
+
+ set_aabb(_aabb);
+
+#else
+ DVector<_VolumeSW_BVH_Element> bvh_array;
+ bvh_array.resize( src_face_count );
+
+ DVector<_VolumeSW_BVH_Element>::Write bvhw = bvh_array.write();
+ _VolumeSW_BVH_Element *bvh_arrayw=bvhw.ptr();
+
+ faces.resize(src_face_count);
+ DVector<Face>::Write w = faces.write();
+ Face *facesw=w.ptr();
+
+ vertices.resize( src_face_count*3 );
+
+ DVector<Vector3>::Write vw = vertices.write();
+ Vector3 *verticesw=vw.ptr();
+
+ AABB _aabb;
+
+
+ for(int i=0;i<src_face_count;i++) {
+
+ Face3 face( facesr[i*3+0], facesr[i*3+1], facesr[i*3+2] );
+
+ bvh_arrayw[i].aabb=face.get_aabb();
+ bvh_arrayw[i].center = bvh_arrayw[i].aabb.pos + bvh_arrayw[i].aabb.size * 0.5;
+ bvh_arrayw[i].face_index=i;
+ facesw[i].indices[0]=i*3+0;
+ facesw[i].indices[1]=i*3+1;
+ facesw[i].indices[2]=i*3+2;
+ facesw[i].normal=face.get_plane().normal;
+ verticesw[i*3+0]=face.vertex[0];
+ verticesw[i*3+1]=face.vertex[1];
+ verticesw[i*3+2]=face.vertex[2];
+ if (i==0)
+ _aabb=bvh_arrayw[i].aabb;
+ else
+ _aabb.merge_with(bvh_arrayw[i].aabb);
+
+ }
+
+ w=DVector<Face>::Write();
+ vw=DVector<Vector3>::Write();
+
+ int count=0;
+ _VolumeSW_BVH *bvh_tree=_volume_sw_build_bvh(bvh_arrayw,src_face_count,count);
+
+ bvh.resize( count+1 );
+
+ DVector<BVH>::Write bvhw2 = bvh.write();
+ BVH*bvh_arrayw2=bvhw2.ptr();
+
+ int idx=0;
+ _fill_bvh(bvh_tree,bvh_arrayw2,idx);
+
+ configure(_aabb); // this type of shape has no margin
+
+
+#endif
+}
+
+
+void ConcavePolygonShapeSW::set_data(const Variant& p_data) {
+
+
+ _setup(p_data);
+}
+
+Variant ConcavePolygonShapeSW::get_data() const {
+
+ return get_faces();
+}
+
+ConcavePolygonShapeSW::ConcavePolygonShapeSW() {
+
+
+}
+
+
+
+/* HEIGHT MAP SHAPE */
+
+DVector<float> HeightMapShapeSW::get_heights() const {
+
+ return heights;
+}
+int HeightMapShapeSW::get_width() const {
+
+ return width;
+}
+int HeightMapShapeSW::get_depth() const {
+
+ return depth;
+}
+float HeightMapShapeSW::get_cell_size() const {
+
+ return cell_size;
+}
+
+
+void HeightMapShapeSW::project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const {
+
+ //not very useful, but not very used either
+ p_transform.xform(get_aabb()).project_range_in_plane( Plane(p_normal,0),r_min,r_max );
+
+}
+
+Vector3 HeightMapShapeSW::get_support(const Vector3& p_normal) const {
+
+
+ //not very useful, but not very used either
+ return get_aabb().get_support(p_normal);
+
+}
+
+bool HeightMapShapeSW::intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const {
+
+
+ return false;
+}
+
+
+void HeightMapShapeSW::cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const {
+
+
+
+}
+
+
+Vector3 HeightMapShapeSW::get_moment_of_inertia(float p_mass) const {
+
+
+ // use crappy AABB approximation
+ Vector3 extents=get_aabb().size*0.5;
+
+ return Vector3(
+ (p_mass/3.0) * (extents.y*extents.y + extents.z*extents.z),
+ (p_mass/3.0) * (extents.x*extents.x + extents.z*extents.z),
+ (p_mass/3.0) * (extents.y*extents.y + extents.y*extents.y)
+ );
+}
+
+
+void HeightMapShapeSW::_setup(DVector<real_t> p_heights,int p_width,int p_depth,real_t p_cell_size) {
+
+ heights=p_heights;
+ width=p_width;
+ depth=p_depth;;
+ cell_size=p_cell_size;
+
+ DVector<real_t>::Read r = heights. read();
+
+ AABB aabb;
+
+ for(int i=0;i<depth;i++) {
+
+ for(int j=0;j<width;j++) {
+
+ float h = r[i*width+j];
+
+ Vector3 pos( j*cell_size, h, i*cell_size );
+ if (i==0 || j==0)
+ aabb.pos=pos;
+ else
+ aabb.expand_to(pos);
+
+ }
+ }
+
+
+ configure(aabb);
+}
+
+void HeightMapShapeSW::set_data(const Variant& p_data) {
+
+ ERR_FAIL_COND( p_data.get_type()!=Variant::DICTIONARY );
+ Dictionary d=p_data;
+ ERR_FAIL_COND( !d.has("width") );
+ ERR_FAIL_COND( !d.has("depth") );
+ ERR_FAIL_COND( !d.has("cell_size") );
+ ERR_FAIL_COND( !d.has("heights") );
+
+ int width=d["width"];
+ int depth=d["depth"];
+ float cell_size=d["cell_size"];
+ DVector<float> heights=d["heights"];
+
+ ERR_FAIL_COND( width<= 0);
+ ERR_FAIL_COND( depth<= 0);
+ ERR_FAIL_COND( cell_size<= CMP_EPSILON);
+ ERR_FAIL_COND( heights.size() != (width*depth) );
+ _setup(heights, width, depth, cell_size );
+
+}
+
+Variant HeightMapShapeSW::get_data() const {
+
+ ERR_FAIL_V(Variant());
+
+}
+
+HeightMapShapeSW::HeightMapShapeSW() {
+
+ width=0;
+ depth=0;
+ cell_size=0;
+}
+
+
+
diff --git a/servers/physics/shape_sw.h b/servers/physics/shape_sw.h new file mode 100644 index 0000000000..890d6d8741 --- /dev/null +++ b/servers/physics/shape_sw.h @@ -0,0 +1,430 @@ +/*************************************************************************/ +/* shape_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef SHAPE_SW_H
+#define SHAPE_SW_H
+
+#include "servers/physics_server.h"
+#include "bsp_tree.h"
+#include "geometry.h"
+/*
+
+SHAPE_LINE, ///< plane:"plane"
+SHAPE_SEGMENT, ///< float:"length"
+SHAPE_CIRCLE, ///< float:"radius"
+SHAPE_RECTANGLE, ///< vec3:"extents"
+SHAPE_CONVEX_POLYGON, ///< array of planes:"planes"
+SHAPE_CONCAVE_POLYGON, ///< Vector3 array:"triangles" , or Dictionary with "indices" (int array) and "triangles" (Vector3 array)
+SHAPE_CUSTOM, ///< Server-Implementation based custom shape, calling shape_create() with this value will result in an error
+
+*/
+
+class ShapeSW;
+
+class ShapeOwnerSW {
+public:
+
+ virtual void _shape_changed()=0;
+ virtual void remove_shape(ShapeSW *p_shape)=0;
+
+ virtual ~ShapeOwnerSW() {}
+};
+
+
+class ShapeSW {
+
+ RID self;
+ AABB aabb;
+ bool configured;
+ real_t custom_bias;
+
+ Map<ShapeOwnerSW*,int> owners;
+protected:
+
+ void configure(const AABB& p_aabb);
+public:
+
+ enum {
+ MAX_SUPPORTS=8
+ };
+
+ _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+ _FORCE_INLINE_ RID get_self() const {return self; }
+
+ virtual PhysicsServer::ShapeType get_type() const=0;
+
+ _FORCE_INLINE_ AABB get_aabb() const { return aabb; }
+ _FORCE_INLINE_ bool is_configured() const { return configured; }
+
+ virtual bool is_concave() const { return false; }
+
+ virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const=0;
+ virtual Vector3 get_support(const Vector3& p_normal) const;
+ virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const=0;
+
+ virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_point, Vector3 &r_normal) const=0;
+ virtual Vector3 get_moment_of_inertia(float p_mass) const=0;
+
+ virtual void set_data(const Variant& p_data)=0;
+ virtual Variant get_data() const=0;
+
+ _FORCE_INLINE_ void set_custom_bias(real_t p_bias) { custom_bias=p_bias; }
+ _FORCE_INLINE_ real_t get_custom_bias() const { return custom_bias; }
+
+ void add_owner(ShapeOwnerSW *p_owner);
+ void remove_owner(ShapeOwnerSW *p_owner);
+ bool is_owner(ShapeOwnerSW *p_owner) const;
+ const Map<ShapeOwnerSW*,int>& get_owners() const;
+
+ ShapeSW();
+ virtual ~ShapeSW();
+};
+
+
+class ConcaveShapeSW : public ShapeSW {
+
+public:
+
+ virtual bool is_concave() const { return true; }
+ typedef void (*Callback)(void* p_userdata,ShapeSW *p_convex);
+ virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; }
+
+ virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const=0;
+
+ ConcaveShapeSW() {}
+};
+
+class PlaneShapeSW : public ShapeSW {
+
+ Plane plane;
+
+ void _setup(const Plane& p_plane);
+public:
+
+ Plane get_plane() const;
+
+ virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_PLANE; }
+ virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+ virtual Vector3 get_support(const Vector3& p_normal) const;
+ virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const { r_amount=0; }
+
+ virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
+ virtual Vector3 get_moment_of_inertia(float p_mass) const;
+
+ virtual void set_data(const Variant& p_data);
+ virtual Variant get_data() const;
+
+ PlaneShapeSW();
+};
+
+class RayShapeSW : public ShapeSW {
+
+ float length;
+
+ void _setup(float p_length);
+public:
+
+ float get_length() const;
+
+ virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_RAY; }
+ virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+ virtual Vector3 get_support(const Vector3& p_normal) const;
+ virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
+
+ virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
+ virtual Vector3 get_moment_of_inertia(float p_mass) const;
+
+ virtual void set_data(const Variant& p_data);
+ virtual Variant get_data() const;
+
+ RayShapeSW();
+};
+
+class SphereShapeSW : public ShapeSW {
+
+ real_t radius;
+
+ void _setup(real_t p_radius);
+public:
+
+ real_t get_radius() const;
+
+ virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_SPHERE; }
+
+ virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+ virtual Vector3 get_support(const Vector3& p_normal) const;
+ virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
+ virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
+ virtual Vector3 get_moment_of_inertia(float p_mass) const;
+
+ virtual void set_data(const Variant& p_data);
+ virtual Variant get_data() const;
+
+ SphereShapeSW();
+};
+
+class BoxShapeSW : public ShapeSW {
+
+ Vector3 half_extents;
+ void _setup(const Vector3& p_half_extents);
+public:
+
+ _FORCE_INLINE_ Vector3 get_half_extents() const { return half_extents; }
+
+ virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_BOX; }
+
+ virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+ virtual Vector3 get_support(const Vector3& p_normal) const;
+ virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
+ virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
+ virtual Vector3 get_moment_of_inertia(float p_mass) const;
+
+ virtual void set_data(const Variant& p_data);
+ virtual Variant get_data() const;
+
+ BoxShapeSW();
+};
+
+class CapsuleShapeSW : public ShapeSW {
+
+ real_t height;
+ real_t radius;
+
+
+ void _setup(real_t p_height,real_t p_radius);
+public:
+
+ _FORCE_INLINE_ real_t get_height() const { return height; }
+ _FORCE_INLINE_ real_t get_radius() const { return radius; }
+
+ virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CAPSULE; }
+
+ virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+ virtual Vector3 get_support(const Vector3& p_normal) const;
+ virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
+ virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
+ virtual Vector3 get_moment_of_inertia(float p_mass) const;
+
+ virtual void set_data(const Variant& p_data);
+ virtual Variant get_data() const;
+
+ CapsuleShapeSW();
+};
+
+struct ConvexPolygonShapeSW : public ShapeSW {
+
+ Geometry::MeshData mesh;
+
+ void _setup(const Vector<Vector3>& p_vertices);
+public:
+
+ const Geometry::MeshData& get_mesh() const { return mesh; }
+
+ virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONVEX_POLYGON; }
+
+ virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+ virtual Vector3 get_support(const Vector3& p_normal) const;
+ virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
+ virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
+ virtual Vector3 get_moment_of_inertia(float p_mass) const;
+
+ virtual void set_data(const Variant& p_data);
+ virtual Variant get_data() const;
+
+ ConvexPolygonShapeSW();
+
+};
+
+
+struct _VolumeSW_BVH;
+struct FaceShapeSW;
+
+struct ConcavePolygonShapeSW : public ConcaveShapeSW {
+ // always a trimesh
+
+ struct Face {
+
+ Vector3 normal;
+ int indices[3];
+ };
+
+ DVector<Face> faces;
+ DVector<Vector3> vertices;
+
+ struct BVH {
+
+ AABB aabb;
+ int left;
+ int right;
+
+ int face_index;
+ };
+
+ DVector<BVH> bvh;
+
+ struct _CullParams {
+
+ AABB aabb;
+ Callback callback;
+ void *userdata;
+ const Face *faces;
+ const Vector3 *vertices;
+ const BVH *bvh;
+ FaceShapeSW *face;
+ };
+
+ struct _SegmentCullParams {
+
+ Vector3 from;
+ Vector3 to;
+ const Face *faces;
+ const Vector3 *vertices;
+ const BVH *bvh;
+
+ Vector3 result;
+ Vector3 normal;
+ real_t min_d;
+ int collisions;
+
+ };
+
+ void _cull_segment(int p_idx,_SegmentCullParams *p_params) const;
+ void _cull(int p_idx,_CullParams *p_params) const;
+
+ void _fill_bvh(_VolumeSW_BVH* p_bvh_tree,BVH* p_bvh_array,int& p_idx);
+
+
+ void _setup(DVector<Vector3> p_faces);
+public:
+
+ DVector<Vector3> get_faces() const;
+
+ virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; }
+
+ virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+ virtual Vector3 get_support(const Vector3& p_normal) const;
+
+ virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
+ virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const;
+
+ virtual Vector3 get_moment_of_inertia(float p_mass) const;
+
+ virtual void set_data(const Variant& p_data);
+ virtual Variant get_data() const;
+
+ ConcavePolygonShapeSW();
+
+};
+
+
+struct HeightMapShapeSW : public ConcaveShapeSW {
+
+ DVector<real_t> heights;
+ int width;
+ int depth;
+ float cell_size;
+
+// void _cull_segment(int p_idx,_SegmentCullParams *p_params) const;
+// void _cull(int p_idx,_CullParams *p_params) const;
+
+ void _setup(DVector<float> p_heights,int p_width,int p_depth,float p_cell_size);
+public:
+
+ DVector<real_t> get_heights() const;
+ int get_width() const;
+ int get_depth() const;
+ float get_cell_size() const;
+
+ virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_HEIGHTMAP; }
+
+ virtual void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+ virtual Vector3 get_support(const Vector3& p_normal) const;
+ virtual bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
+ virtual void cull(const AABB& p_local_aabb,Callback p_callback,void* p_userdata) const;
+
+ virtual Vector3 get_moment_of_inertia(float p_mass) const;
+
+ virtual void set_data(const Variant& p_data);
+ virtual Variant get_data() const;
+
+ HeightMapShapeSW();
+
+};
+
+//used internally
+struct FaceShapeSW : public ShapeSW {
+
+ Vector3 normal; //cache
+ Vector3 vertex[3];
+
+ virtual PhysicsServer::ShapeType get_type() const { return PhysicsServer::SHAPE_CONCAVE_POLYGON; }
+
+ const Vector3& get_vertex(int p_idx) const { return vertex[p_idx]; }
+
+ void project_range(const Vector3& p_normal, const Transform& p_transform, real_t &r_min, real_t &r_max) const;
+ Vector3 get_support(const Vector3& p_normal) const;
+ virtual void get_supports(const Vector3& p_normal,int p_max,Vector3 *r_supports,int & r_amount) const;
+ bool intersect_segment(const Vector3& p_begin,const Vector3& p_end,Vector3 &r_result, Vector3 &r_normal) const;
+
+ Vector3 get_moment_of_inertia(float p_mass) const;
+
+ virtual void set_data(const Variant& p_data) {}
+ virtual Variant get_data() const { return Variant(); }
+
+ FaceShapeSW();
+};
+
+
+
+
+
+struct _ShapeTestConvexBSPSW {
+
+ const BSP_Tree *bsp;
+ const ShapeSW *shape;
+ Transform transform;
+
+ _FORCE_INLINE_ void project_range(const Vector3& p_normal, real_t& r_min, real_t& r_max) const {
+
+ shape->project_range(p_normal,transform,r_min,r_max);
+ }
+
+};
+
+
+
+
+#endif // SHAPESW_H
diff --git a/servers/physics/space_sw.cpp b/servers/physics/space_sw.cpp new file mode 100644 index 0000000000..ca3aa7e1f5 --- /dev/null +++ b/servers/physics/space_sw.cpp @@ -0,0 +1,429 @@ +/*************************************************************************/ +/* space_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "space_sw.h"
+#include "collision_solver_sw.h"
+#include "physics_server_sw.h"
+
+
+bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude,uint32_t p_user_mask) {
+
+
+ ERR_FAIL_COND_V(space->locked,false);
+
+ Vector3 begin,end;
+ Vector3 normal;
+ begin=p_from;
+ end=p_to;
+ normal=(end-begin).normalized();
+
+
+ int amount = space->broadphase->cull_segment(begin,end,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+
+ //todo, create another array tha references results, compute AABBs and check closest point to ray origin, sort, and stop evaluating results when beyond first collision
+
+ bool collided=false;
+ Vector3 res_point,res_normal;
+ int res_shape;
+ const CollisionObjectSW *res_obj;
+ real_t min_d=1e10;
+
+
+ for(int i=0;i<amount;i++) {
+
+ if (space->intersection_query_results[i]->get_type()==CollisionObjectSW::TYPE_AREA)
+ continue; //ignore area
+
+ if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+ continue;
+
+ const CollisionObjectSW *col_obj=space->intersection_query_results[i];
+
+ int shape_idx=space->intersection_query_subindex_results[i];
+ Transform inv_xform = col_obj->get_shape_inv_transform(shape_idx) * col_obj->get_inv_transform();
+
+ Vector3 local_from = inv_xform.xform(begin);
+ Vector3 local_to = inv_xform.xform(end);
+
+ const ShapeSW *shape = col_obj->get_shape(shape_idx);
+
+ Vector3 shape_point,shape_normal;
+
+ if (shape->intersect_segment(local_from,local_to,shape_point,shape_normal)) {
+
+ Transform xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
+ shape_point=xform.xform(shape_point);
+
+ real_t ld = normal.dot(shape_point);
+
+
+ if (ld<min_d) {
+
+ min_d=ld;
+ res_point=shape_point;
+ res_normal=inv_xform.basis.xform_inv(shape_normal).normalized();
+ res_shape=shape_idx;
+ res_obj=col_obj;
+ collided=true;
+ }
+ }
+
+ }
+
+ if (!collided)
+ return false;
+
+
+ r_result.collider_id=res_obj->get_instance_id();
+ if (r_result.collider_id!=0)
+ r_result.collider=ObjectDB::get_instance(r_result.collider_id);
+ r_result.normal=res_normal;
+ r_result.position=res_point;
+ r_result.rid=res_obj->get_self();
+ r_result.shape=res_shape;
+
+ return true;
+
+}
+
+
+int PhysicsDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Transform& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude,uint32_t p_user_mask) {
+
+ if (p_result_max<=0)
+ return 0;
+
+ ShapeSW *shape = static_cast<PhysicsServerSW*>(PhysicsServer::get_singleton())->shape_owner.get(p_shape);
+ ERR_FAIL_COND_V(!shape,0);
+
+ AABB aabb = p_xform.xform(shape->get_aabb());
+
+ int amount = space->broadphase->cull_aabb(aabb,space->intersection_query_results,SpaceSW::INTERSECTION_QUERY_MAX,space->intersection_query_subindex_results);
+
+ bool collided=false;
+ int cc=0;
+
+ //Transform ai = p_xform.affine_inverse();
+
+ for(int i=0;i<amount;i++) {
+
+ if (cc>=p_result_max)
+ break;
+
+ if (space->intersection_query_results[i]->get_type()==CollisionObjectSW::TYPE_AREA)
+ continue; //ignore area
+
+ if (p_exclude.has( space->intersection_query_results[i]->get_self()))
+ continue;
+
+
+ const CollisionObjectSW *col_obj=space->intersection_query_results[i];
+ int shape_idx=space->intersection_query_subindex_results[i];
+
+ if (!CollisionSolverSW::solve_static(shape,p_xform,col_obj->get_shape(shape_idx),col_obj->get_transform() * col_obj->get_shape_transform(shape_idx), NULL,NULL,NULL))
+ continue;
+
+ r_results[cc].collider_id=col_obj->get_instance_id();
+ if (r_results[cc].collider_id!=0)
+ r_results[cc].collider=ObjectDB::get_instance(r_results[cc].collider_id);
+ r_results[cc].rid=col_obj->get_self();
+ r_results[cc].shape=shape_idx;
+
+ cc++;
+
+ }
+
+ return cc;
+
+}
+
+PhysicsDirectSpaceStateSW::PhysicsDirectSpaceStateSW() {
+
+
+ space=NULL;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+
+
+
+
+
+
+
+
+
+void* SpaceSW::_broadphase_pair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_self) {
+
+ CollisionObjectSW::Type type_A=A->get_type();
+ CollisionObjectSW::Type type_B=B->get_type();
+ if (type_A>type_B) {
+
+ SWAP(A,B);
+ SWAP(p_subindex_A,p_subindex_B);
+ SWAP(type_A,type_B);
+ }
+
+ SpaceSW *self = (SpaceSW*)p_self;
+
+ if (type_A==CollisionObjectSW::TYPE_AREA) {
+
+
+ ERR_FAIL_COND_V(type_B!=CollisionObjectSW::TYPE_BODY,NULL);
+ AreaSW *area=static_cast<AreaSW*>(A);
+ BodySW *body=static_cast<BodySW*>(B);
+
+
+ AreaPairSW *area_pair = memnew(AreaPairSW(body,p_subindex_B,area,p_subindex_A) );
+
+ return area_pair;
+ } else {
+
+
+ BodyPairSW *b = memnew( BodyPairSW((BodySW*)A,p_subindex_A,(BodySW*)B,p_subindex_B) );
+ return b;
+
+ }
+
+ return NULL;
+}
+
+void SpaceSW::_broadphase_unpair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_data,void *p_self) {
+
+
+
+ SpaceSW *self = (SpaceSW*)p_self;
+ ConstraintSW *c = (ConstraintSW*)p_data;
+ memdelete(c);
+}
+
+
+const SelfList<BodySW>::List& SpaceSW::get_active_body_list() const {
+
+ return active_list;
+}
+void SpaceSW::body_add_to_active_list(SelfList<BodySW>* p_body) {
+
+ active_list.add(p_body);
+}
+void SpaceSW::body_remove_from_active_list(SelfList<BodySW>* p_body) {
+
+ active_list.remove(p_body);
+
+}
+
+void SpaceSW::body_add_to_inertia_update_list(SelfList<BodySW>* p_body) {
+
+
+ inertia_update_list.add(p_body);
+}
+
+void SpaceSW::body_remove_from_inertia_update_list(SelfList<BodySW>* p_body) {
+
+ inertia_update_list.remove(p_body);
+}
+
+BroadPhaseSW *SpaceSW::get_broadphase() {
+
+ return broadphase;
+}
+
+void SpaceSW::add_object(CollisionObjectSW *p_object) {
+
+ ERR_FAIL_COND( objects.has(p_object) );
+ objects.insert(p_object);
+}
+
+void SpaceSW::remove_object(CollisionObjectSW *p_object) {
+
+ ERR_FAIL_COND( !objects.has(p_object) );
+ objects.erase(p_object);
+}
+
+const Set<CollisionObjectSW*> &SpaceSW::get_objects() const {
+
+ return objects;
+}
+
+void SpaceSW::body_add_to_state_query_list(SelfList<BodySW>* p_body) {
+
+ state_query_list.add(p_body);
+}
+void SpaceSW::body_remove_from_state_query_list(SelfList<BodySW>* p_body) {
+
+ state_query_list.remove(p_body);
+}
+
+void SpaceSW::area_add_to_monitor_query_list(SelfList<AreaSW>* p_area) {
+
+ monitor_query_list.add(p_area);
+}
+void SpaceSW::area_remove_from_monitor_query_list(SelfList<AreaSW>* p_area) {
+
+ monitor_query_list.remove(p_area);
+}
+
+void SpaceSW::area_add_to_moved_list(SelfList<AreaSW>* p_area) {
+
+ area_moved_list.add(p_area);
+}
+
+void SpaceSW::area_remove_from_moved_list(SelfList<AreaSW>* p_area) {
+
+ area_moved_list.remove(p_area);
+}
+
+const SelfList<AreaSW>::List& SpaceSW::get_moved_area_list() const {
+
+ return area_moved_list;
+}
+
+
+
+
+void SpaceSW::call_queries() {
+
+ while(state_query_list.first()) {
+
+ BodySW * b = state_query_list.first()->self();
+ b->call_queries();
+ state_query_list.remove(state_query_list.first());
+ }
+
+ while(monitor_query_list.first()) {
+
+ AreaSW * a = monitor_query_list.first()->self();
+ a->call_queries();
+ monitor_query_list.remove(monitor_query_list.first());
+ }
+
+}
+
+void SpaceSW::setup() {
+
+
+ while(inertia_update_list.first()) {
+ inertia_update_list.first()->self()->update_inertias();
+ inertia_update_list.remove(inertia_update_list.first());
+ }
+
+
+}
+
+void SpaceSW::update() {
+
+ broadphase->update();
+
+}
+
+
+void SpaceSW::set_param(PhysicsServer::SpaceParameter p_param, real_t p_value) {
+
+ switch(p_param) {
+
+ case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: contact_recycle_radius=p_value; break;
+ case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: contact_max_separation=p_value; break;
+ case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: contact_max_allowed_penetration=p_value; break;
+ case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: body_linear_velocity_sleep_treshold=p_value; break;
+ case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: body_angular_velocity_sleep_treshold=p_value; break;
+ case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: body_time_to_sleep=p_value; break;
+ case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: body_angular_velocity_damp_ratio=p_value; break;
+ case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: constraint_bias=p_value; break;
+ }
+}
+
+real_t SpaceSW::get_param(PhysicsServer::SpaceParameter p_param) const {
+
+ switch(p_param) {
+
+ case PhysicsServer::SPACE_PARAM_CONTACT_RECYCLE_RADIUS: return contact_recycle_radius;
+ case PhysicsServer::SPACE_PARAM_CONTACT_MAX_SEPARATION: return contact_max_separation;
+ case PhysicsServer::SPACE_PARAM_BODY_MAX_ALLOWED_PENETRATION: return contact_max_allowed_penetration;
+ case PhysicsServer::SPACE_PARAM_BODY_LINEAR_VELOCITY_SLEEP_TRESHOLD: return body_linear_velocity_sleep_treshold;
+ case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_SLEEP_TRESHOLD: return body_angular_velocity_sleep_treshold;
+ case PhysicsServer::SPACE_PARAM_BODY_TIME_TO_SLEEP: return body_time_to_sleep;
+ case PhysicsServer::SPACE_PARAM_BODY_ANGULAR_VELOCITY_DAMP_RATIO: return body_angular_velocity_damp_ratio;
+ case PhysicsServer::SPACE_PARAM_CONSTRAINT_DEFAULT_BIAS: return constraint_bias;
+ }
+ return 0;
+}
+
+void SpaceSW::lock() {
+
+ locked=true;
+}
+
+void SpaceSW::unlock() {
+
+ locked=false;
+}
+
+bool SpaceSW::is_locked() const {
+
+ return locked;
+}
+
+PhysicsDirectSpaceStateSW *SpaceSW::get_direct_state() {
+
+ return direct_access;
+}
+
+SpaceSW::SpaceSW() {
+
+
+ locked=false;
+ contact_recycle_radius=0.01;
+ contact_max_separation=0.05;
+ contact_max_allowed_penetration= 0.01;
+
+ constraint_bias = 0.01;
+ body_linear_velocity_sleep_treshold=0.01;
+ body_angular_velocity_sleep_treshold=(8.0 / 180.0 * Math_PI);
+ body_time_to_sleep=0.5;
+ body_angular_velocity_damp_ratio=10;
+
+
+ broadphase = BroadPhaseSW::create_func();
+ broadphase->set_pair_callback(_broadphase_pair,this);
+ broadphase->set_unpair_callback(_broadphase_unpair,this);
+ area=NULL;
+
+ direct_access = memnew( PhysicsDirectSpaceStateSW );
+ direct_access->space=this;
+}
+
+SpaceSW::~SpaceSW() {
+
+ memdelete(broadphase);
+ memdelete( direct_access );
+}
+
+
+
diff --git a/servers/physics/space_sw.h b/servers/physics/space_sw.h new file mode 100644 index 0000000000..202c7ccbd2 --- /dev/null +++ b/servers/physics/space_sw.h @@ -0,0 +1,157 @@ +/*************************************************************************/ +/* space_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef SPACE_SW_H
+#define SPACE_SW_H
+
+#include "typedefs.h"
+#include "hash_map.h"
+#include "body_sw.h"
+#include "area_sw.h"
+#include "body_pair_sw.h"
+#include "area_pair_sw.h"
+#include "broad_phase_sw.h"
+#include "collision_object_sw.h"
+
+
+class PhysicsDirectSpaceStateSW : public PhysicsDirectSpaceState {
+
+ OBJ_TYPE( PhysicsDirectSpaceStateSW, PhysicsDirectSpaceState );
+public:
+
+ SpaceSW *space;
+
+ bool intersect_ray(const Vector3& p_from, const Vector3& p_to,RayResult &r_result,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
+ int intersect_shape(const RID& p_shape, const Transform& p_xform,ShapeResult *r_results,int p_result_max,const Set<RID>& p_exclude=Set<RID>(),uint32_t p_user_mask=0);
+
+ PhysicsDirectSpaceStateSW();
+};
+
+
+
+class SpaceSW {
+
+
+ PhysicsDirectSpaceStateSW *direct_access;
+ RID self;
+
+ BroadPhaseSW *broadphase;
+ SelfList<BodySW>::List active_list;
+ SelfList<BodySW>::List inertia_update_list;
+ SelfList<BodySW>::List state_query_list;
+ SelfList<AreaSW>::List monitor_query_list;
+ SelfList<AreaSW>::List area_moved_list;
+
+ static void* _broadphase_pair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_self);
+ static void _broadphase_unpair(CollisionObjectSW *A,int p_subindex_A,CollisionObjectSW *B,int p_subindex_B,void *p_data,void *p_self);
+
+ Set<CollisionObjectSW*> objects;
+
+ AreaSW *area;
+
+ real_t contact_recycle_radius;
+ real_t contact_max_separation;
+ real_t contact_max_allowed_penetration;
+ real_t constraint_bias;
+
+ enum {
+
+ INTERSECTION_QUERY_MAX=2048
+ };
+
+ CollisionObjectSW *intersection_query_results[INTERSECTION_QUERY_MAX];
+ int intersection_query_subindex_results[INTERSECTION_QUERY_MAX];
+
+ float body_linear_velocity_sleep_treshold;
+ float body_angular_velocity_sleep_treshold;
+ float body_time_to_sleep;
+ float body_angular_velocity_damp_ratio;
+
+ bool locked;
+
+friend class PhysicsDirectSpaceStateSW;
+
+public:
+
+ _FORCE_INLINE_ void set_self(const RID& p_self) { self=p_self; }
+ _FORCE_INLINE_ RID get_self() const { return self; }
+
+ void set_default_area(AreaSW *p_area) { area=p_area; }
+ AreaSW *get_default_area() const { return area; }
+
+ const SelfList<BodySW>::List& get_active_body_list() const;
+ void body_add_to_active_list(SelfList<BodySW>* p_body);
+ void body_remove_from_active_list(SelfList<BodySW>* p_body);
+ void body_add_to_inertia_update_list(SelfList<BodySW>* p_body);
+ void body_remove_from_inertia_update_list(SelfList<BodySW>* p_body);
+
+ void body_add_to_state_query_list(SelfList<BodySW>* p_body);
+ void body_remove_from_state_query_list(SelfList<BodySW>* p_body);
+
+ void area_add_to_monitor_query_list(SelfList<AreaSW>* p_area);
+ void area_remove_from_monitor_query_list(SelfList<AreaSW>* p_area);
+ void area_add_to_moved_list(SelfList<AreaSW>* p_area);
+ void area_remove_from_moved_list(SelfList<AreaSW>* p_area);
+ const SelfList<AreaSW>::List& get_moved_area_list() const;
+
+ BroadPhaseSW *get_broadphase();
+
+ void add_object(CollisionObjectSW *p_object);
+ void remove_object(CollisionObjectSW *p_object);
+ const Set<CollisionObjectSW*> &get_objects() const;
+
+ _FORCE_INLINE_ real_t get_contact_recycle_radius() const { return contact_recycle_radius; }
+ _FORCE_INLINE_ real_t get_contact_max_separation() const { return contact_max_separation; }
+ _FORCE_INLINE_ real_t get_contact_max_allowed_penetration() const { return contact_max_allowed_penetration; }
+ _FORCE_INLINE_ real_t get_constraint_bias() const { return constraint_bias; }
+ _FORCE_INLINE_ real_t get_body_linear_velocity_sleep_treshold() const { return body_linear_velocity_sleep_treshold; }
+ _FORCE_INLINE_ real_t get_body_angular_velocity_sleep_treshold() const { return body_angular_velocity_sleep_treshold; }
+ _FORCE_INLINE_ real_t get_body_time_to_sleep() const { return body_time_to_sleep; }
+ _FORCE_INLINE_ real_t get_body_angular_velocity_damp_ratio() const { return body_angular_velocity_damp_ratio; }
+
+
+ void update();
+ void setup();
+ void call_queries();
+
+
+ bool is_locked() const;
+ void lock();
+ void unlock();
+
+ void set_param(PhysicsServer::SpaceParameter p_param, real_t p_value);
+ real_t get_param(PhysicsServer::SpaceParameter p_param) const;
+
+ PhysicsDirectSpaceStateSW *get_direct_state();
+
+ SpaceSW();
+ ~SpaceSW();
+};
+
+
+#endif // SPACE__SW_H
diff --git a/servers/physics/step_sw.cpp b/servers/physics/step_sw.cpp new file mode 100644 index 0000000000..b7815d2250 --- /dev/null +++ b/servers/physics/step_sw.cpp @@ -0,0 +1,237 @@ +/*************************************************************************/ +/* step_sw.cpp */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#include "step_sw.h" + + +void StepSW::_populate_island(BodySW* p_body,BodySW** p_island,ConstraintSW **p_constraint_island) { + + p_body->set_island_step(_step); + p_body->set_island_next(*p_island); + *p_island=p_body; + + for(Map<ConstraintSW*,int>::Element *E=p_body->get_constraint_map().front();E;E=E->next()) { + + ConstraintSW *c=(ConstraintSW*)E->key(); + if (c->get_island_step()==_step) + continue; //already processed + c->set_island_step(_step); + c->set_island_next(*p_constraint_island); + *p_constraint_island=c; + + + for(int i=0;i<c->get_body_count();i++) { + if (i==E->get()) + continue; + BodySW *b = c->get_body_ptr()[i]; + if (b->get_island_step()==_step || b->get_mode()==PhysicsServer::BODY_MODE_STATIC) + continue; //no go + _populate_island(c->get_body_ptr()[i],p_island,p_constraint_island); + } + } +} + +void StepSW::_setup_island(ConstraintSW *p_island,float p_delta) { + + ConstraintSW *ci=p_island; + while(ci) { + bool process = ci->setup(p_delta); + //todo remove from island if process fails + ci=ci->get_island_next(); + } +} + +void StepSW::_solve_island(ConstraintSW *p_island,int p_iterations,float p_delta){ + + for(int i=0;i<p_iterations;i++) { + + ConstraintSW *ci=p_island; + while(ci) { + ci->solve(p_delta); + ci=ci->get_island_next(); + } + } +} + +void StepSW::_check_suspend(BodySW *p_island,float p_delta) { + + + bool can_sleep=true; + + BodySW *b = p_island; + while(b) { + + if (b->get_mode()==PhysicsServer::BODY_MODE_STATIC) + continue; //ignore for static + + if (!b->sleep_test(p_delta)) + can_sleep=false; + + b=b->get_island_next(); + } + + //put all to sleep or wake up everyoen + + b = p_island; + while(b) { + + if (b->get_mode()==PhysicsServer::BODY_MODE_STATIC) + continue; //ignore for static + + bool active = b->is_active(); + + if (active==can_sleep) + b->set_active(!can_sleep); + + b=b->get_island_next(); + } +} + +void StepSW::step(SpaceSW* p_space,float p_delta,int p_iterations) { + + p_space->lock(); // can't access space during this + + p_space->setup(); //update inertias, etc + + const SelfList<BodySW>::List * body_list = &p_space->get_active_body_list(); + + /* INTEGRATE FORCES */ + int active_count=0; + + const SelfList<BodySW>*b = body_list->first(); + while(b) { + + b->self()->integrate_forces(p_delta); + b=b->next(); + active_count++; + } + + + + /* GENERATE CONSTRAINT ISLANDS */ + + BodySW *island_list=NULL; + ConstraintSW *constraint_island_list=NULL; + b = body_list->first(); + + int island_count=0; + + while(b) { + BodySW *body = b->self(); + + + if (body->get_island_step()!=_step) { + + BodySW *island=NULL; + ConstraintSW *constraint_island=NULL; + _populate_island(body,&island,&constraint_island); + + island->set_island_list_next(island_list); + island_list=island; + + if (constraint_island) { + constraint_island->set_island_list_next(constraint_island_list); + constraint_island_list=constraint_island; + island_count++; + } + + } + b=b->next(); + } + + const SelfList<AreaSW>::List &aml = p_space->get_moved_area_list(); + + while(aml.first()) { + for(const Set<ConstraintSW*>::Element *E=aml.first()->self()->get_constraints().front();E;E=E->next()) { + + ConstraintSW*c=E->get(); + if (c->get_island_step()==_step) + continue; + c->set_island_step(_step); + c->set_island_next(NULL); + c->set_island_list_next(constraint_island_list); + constraint_island_list=c; + } + p_space->area_remove_from_moved_list((SelfList<AreaSW>*)aml.first()); //faster to remove here + } + +// print_line("island count: "+itos(island_count)+" active count: "+itos(active_count)); + /* SETUP CONSTRAINT ISLANDS */ + + { + ConstraintSW *ci=constraint_island_list; + while(ci) { + + _setup_island(ci,p_delta); + ci=ci->get_island_list_next(); + } + } + + /* SOLVE CONSTRAINT ISLANDS */ + + { + ConstraintSW *ci=constraint_island_list; + while(ci) { + //iterating each island separatedly improves cache efficiency + _solve_island(ci,p_iterations,p_delta); + ci=ci->get_island_list_next(); + } + } + + /* INTEGRATE VELOCITIES */ + + b = body_list->first(); + while(b) { + + b->self()->integrate_velocities(p_delta); + b=b->next(); + } + + /* SLEEP / WAKE UP ISLANDS */ + + { + BodySW *bi=island_list; + while(bi) { + + _check_suspend(bi,p_delta); + bi=bi->get_island_list_next(); + } + } + + p_space->update(); + p_space->unlock(); + _step++; + + + +} + +StepSW::StepSW() { + + _step=1; +} diff --git a/servers/physics/step_sw.h b/servers/physics/step_sw.h new file mode 100644 index 0000000000..9aaef97920 --- /dev/null +++ b/servers/physics/step_sw.h @@ -0,0 +1,48 @@ +/*************************************************************************/ +/* step_sw.h */ +/*************************************************************************/ +/* This file is part of: */ +/* GODOT ENGINE */ +/* http://www.godotengine.org */ +/*************************************************************************/ +/* Copyright (c) 2007-2014 Juan Linietsky, Ariel Manzur. */ +/* */ +/* Permission is hereby granted, free of charge, to any person obtaining */ +/* a copy of this software and associated documentation files (the */ +/* "Software"), to deal in the Software without restriction, including */ +/* without limitation the rights to use, copy, modify, merge, publish, */ +/* distribute, sublicense, and/or sell copies of the Software, and to */ +/* permit persons to whom the Software is furnished to do so, subject to */ +/* the following conditions: */ +/* */ +/* The above copyright notice and this permission notice shall be */ +/* included in all copies or substantial portions of the Software. */ +/* */ +/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */ +/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */ +/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/ +/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */ +/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */ +/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */ +/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */ +/*************************************************************************/ +#ifndef STEP_SW_H +#define STEP_SW_H + +#include "space_sw.h" + +class StepSW { + + uint64_t _step; + + void _populate_island(BodySW* p_body,BodySW** p_island,ConstraintSW **p_constraint_island); + void _setup_island(ConstraintSW *p_island,float p_delta); + void _solve_island(ConstraintSW *p_island,int p_iterations,float p_delta); + void _check_suspend(BodySW *p_island,float p_delta); +public: + + void step(SpaceSW* p_space,float p_delta,int p_iterations); + StepSW(); +}; + +#endif // STEP__SW_H |