summaryrefslogtreecommitdiff
path: root/servers/physics/body_pair_sw.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics/body_pair_sw.cpp')
-rw-r--r--servers/physics/body_pair_sw.cpp331
1 files changed, 145 insertions, 186 deletions
diff --git a/servers/physics/body_pair_sw.cpp b/servers/physics/body_pair_sw.cpp
index 7fb3def387..555d5f15c5 100644
--- a/servers/physics/body_pair_sw.cpp
+++ b/servers/physics/body_pair_sw.cpp
@@ -28,8 +28,8 @@
/*************************************************************************/
#include "body_pair_sw.h"
#include "collision_solver_sw.h"
-#include "space_sw.h"
#include "os/os.h"
+#include "space_sw.h"
/*
#define NO_ACCUMULATE_IMPULSES
@@ -41,19 +41,17 @@
#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, 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) {
+void BodyPairSW::contact_added_callback(const Vector3 &p_point_A, const Vector3 &p_point_B) {
// check if we already have the contact
@@ -61,40 +59,36 @@ void BodyPairSW::contact_added_callback(const Vector3& p_point_A,const Vector3&
//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);
-
-
+ 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) );
+ 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();
-
-
+ 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();
+ real_t contact_recycle_radius = space->get_contact_recycle_radius();
- for (int i=0;i<contact_count;i++) {
+ for (int i = 0; i < contact_count; i++) {
- Contact& c = contacts[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;
+ 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;
}
}
@@ -104,66 +98,63 @@ void BodyPairSW::contact_added_callback(const Vector3& p_point_A,const Vector3&
// remove the contact with the minimum depth
- int least_deep=-1;
- real_t min_depth=1e10;
+ int least_deep = -1;
+ real_t min_depth = 1e10;
- for (int i=0;i<=contact_count;i++) {
+ for (int i = 0; i <= contact_count; i++) {
- Contact& c = (i==contact_count)?contact:contacts[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 global_B = B->get_transform().basis.xform(c.local_B) + offset_B;
Vector3 axis = global_A - global_B;
- real_t depth = axis.dot( c.normal );
+ real_t depth = axis.dot(c.normal);
- if (depth<min_depth) {
+ if (depth < min_depth) {
- min_depth=depth;
- least_deep=i;
+ min_depth = depth;
+ least_deep = i;
}
}
- ERR_FAIL_COND(least_deep==-1);
+ ERR_FAIL_COND(least_deep == -1);
if (least_deep < contact_count) { //replace the last deep contact by the new one
- contacts[least_deep]=contact;
+ contacts[least_deep] = contact;
}
return;
}
- contacts[new_index]=contact;
+ contacts[new_index] = contact;
- if (new_index==contact_count) {
+ 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++) {
+ real_t contact_max_separation = space->get_contact_max_separation();
+ for (int i = 0; i < contact_count; i++) {
- Contact& c = contacts[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 global_B = B->get_transform().basis.xform(c.local_B) + offset_B;
Vector3 axis = global_A - global_B;
- real_t depth = axis.dot( c.normal );
+ real_t 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) {
+ if ((i + 1) < contact_count) {
// swap with the last one
- SWAP( contacts[i], contacts[ contact_count-1 ] );
-
+ SWAP(contacts[i], contacts[contact_count - 1]);
}
i--;
@@ -172,21 +163,18 @@ void BodyPairSW::validate_contacts() {
}
}
+bool BodyPairSW::_test_ccd(real_t p_step, BodySW *p_A, int p_shape_A, const Transform &p_xform_A, BodySW *p_B, int p_shape_B, const Transform &p_xform_B) {
-bool BodyPairSW::_test_ccd(real_t p_step,BodySW *p_A, int p_shape_A,const Transform& p_xform_A,BodySW *p_B, int p_shape_B,const Transform& p_xform_B) {
-
-
-
- Vector3 motion = p_A->get_linear_velocity()*p_step;
+ Vector3 motion = p_A->get_linear_velocity() * p_step;
real_t mlen = motion.length();
- if (mlen<CMP_EPSILON)
+ if (mlen < CMP_EPSILON)
return false;
Vector3 mnormal = motion / mlen;
- real_t min,max;
- p_A->get_shape(p_shape_A)->project_range(mnormal,p_xform_A,min,max);
- bool fast_object = mlen > (max-min)*0.3; //going too fast in that direction
+ real_t min, max;
+ p_A->get_shape(p_shape_A)->project_range(mnormal, p_xform_A, min, max);
+ bool fast_object = mlen > (max - min) * 0.3; //going too fast in that direction
if (!fast_object) { //did it move enough in this direction to even attempt raycast? let's say it should move more than 1/3 the size of the object in that axis
return false;
@@ -194,35 +182,34 @@ bool BodyPairSW::_test_ccd(real_t p_step,BodySW *p_A, int p_shape_A,const Transf
//cast a segment from support in motion normal, in the same direction of motion by motion length
//support is the worst case collision point, so real collision happened before
- Vector3 s=p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform(mnormal).normalized());
+ Vector3 s = p_A->get_shape(p_shape_A)->get_support(p_xform_A.basis.xform(mnormal).normalized());
Vector3 from = p_xform_A.xform(s);
Vector3 to = from + motion;
Transform from_inv = p_xform_B.affine_inverse();
- Vector3 local_from = from_inv.xform(from-mnormal*mlen*0.1); //start from a little inside the bounding box
+ Vector3 local_from = from_inv.xform(from - mnormal * mlen * 0.1); //start from a little inside the bounding box
Vector3 local_to = from_inv.xform(to);
- Vector3 rpos,rnorm;
- if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from,local_to,rpos,rnorm)) {
+ Vector3 rpos, rnorm;
+ if (!p_B->get_shape(p_shape_B)->intersect_segment(local_from, local_to, rpos, rnorm)) {
return false;
}
//shorten the linear velocity so it does not hit, but gets close enough, next frame will hit softly or soft enough
Vector3 hitpos = p_xform_B.xform(rpos);
- real_t newlen = hitpos.distance_to(from)-(max-min)*0.01;
- p_A->set_linear_velocity((mnormal*newlen)/p_step);
+ real_t newlen = hitpos.distance_to(from) - (max - min) * 0.01;
+ p_A->set_linear_velocity((mnormal * newlen) / p_step);
return true;
}
-
bool BodyPairSW::setup(real_t p_step) {
//cannot collide
- if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported()==0 && B->get_max_contacts_reported()==0)) {
- collided=false;
+ if (!A->test_collision_mask(B) || A->has_exception(B->get_self()) || B->has_exception(A->get_self()) || (A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC && A->get_max_contacts_reported() == 0 && B->get_max_contacts_reported() == 0)) {
+ collided = false;
return false;
}
@@ -231,86 +218,79 @@ bool BodyPairSW::setup(real_t p_step) {
validate_contacts();
Vector3 offset_A = A->get_transform().get_origin();
- Transform xform_Au = Transform(A->get_transform().basis,Vector3());
+ 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;
+ 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;
+ 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) {
//test ccd (currently just a raycast)
- if (A->is_continuous_collision_detection_enabled() && A->get_mode()>PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC) {
- _test_ccd(p_step,A,shape_A,xform_A,B,shape_B,xform_B);
+ if (A->is_continuous_collision_detection_enabled() && A->get_mode() > PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) {
+ _test_ccd(p_step, A, shape_A, xform_A, B, shape_B, xform_B);
}
- if (B->is_continuous_collision_detection_enabled() && B->get_mode()>PhysicsServer::BODY_MODE_KINEMATIC && A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC) {
- _test_ccd(p_step,B,shape_B,xform_B,A,shape_A,xform_A);
+ if (B->is_continuous_collision_detection_enabled() && B->get_mode() > PhysicsServer::BODY_MODE_KINEMATIC && A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC) {
+ _test_ccd(p_step, B, shape_B, xform_B, A, shape_A, xform_A);
}
return false;
}
-
-
real_t max_penetration = space->get_contact_max_allowed_penetration();
real_t bias = (real_t)0.3;
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();
+ 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;
+ bias = (shape_B_ptr->get_custom_bias() + shape_A_ptr->get_custom_bias()) * 0.5;
}
+ real_t inv_dt = 1.0 / p_step;
-
- real_t inv_dt = 1.0/p_step;
-
- for(int i=0;i<contact_count;i++) {
+ for (int i = 0; i < contact_count; i++) {
Contact &c = contacts[i];
- c.active=false;
+ 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;
+ if (depth <= 0) {
+ c.active = false;
continue;
}
- c.active=true;
+ c.active = true;
#ifdef DEBUG_ENABLED
-
if (space->is_debugging_contacts()) {
- space->add_debug_contact(global_A+offset_A);
- space->add_debug_contact(global_B+offset_A);
+ space->add_debug_contact(global_A + offset_A);
+ space->add_debug_contact(global_B + offset_A);
}
#endif
- c.rA = global_A-A->get_center_of_mass();
- c.rB = global_B-B->get_center_of_mass()-offset_B;
+ c.rA = global_A - A->get_center_of_mass();
+ c.rB = global_B - B->get_center_of_mass() - offset_B;
- // contact query reporting...
+// 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 );
@@ -323,30 +303,28 @@ bool BodyPairSW::setup(real_t p_step) {
#endif
if (A->can_report_contacts()) {
- Vector3 crA = 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(),crA);
+ Vector3 crA = 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(), crA);
}
if (B->can_report_contacts()) {
- Vector3 crB = B->get_angular_velocity().cross( c.rB ) + B->get_linear_velocity();
- B->add_contact(global_B,c.normal,depth,shape_B,global_A,shape_A,A->get_instance_id(),A->get_self(),crB);
+ Vector3 crB = B->get_angular_velocity().cross(c.rB) + B->get_linear_velocity();
+ B->add_contact(global_B, c.normal, depth, shape_B, global_A, shape_A, A->get_instance_id(), A->get_self(), crB);
}
- if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode()<=PhysicsServer::BODY_MODE_KINEMATIC)) {
- c.active=false;
- collided=false;
+ if (A->is_shape_set_as_trigger(shape_A) || B->is_shape_set_as_trigger(shape_B) || (A->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC && B->get_mode() <= PhysicsServer::BODY_MODE_KINEMATIC)) {
+ c.active = false;
+ collided = false;
continue;
-
}
-
- c.active=true;
+ 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 ) );
+ 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 ));
+ 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
@@ -354,34 +332,32 @@ bool BodyPairSW::setup(real_t p_step) {
#else
if (depth > max_penetration) {
- c.bias = (depth - max_penetration) * (1.0/(p_step*(1.0/RELAXATION_TIMESTEPS)));
+ c.bias = (depth - max_penetration) * (1.0 / (p_step * (1.0 / RELAXATION_TIMESTEPS)));
} else {
real_t approach = -0.1 * (depth - max_penetration) / (CMP_EPSILON + max_penetration);
- approach = CLAMP( approach, CMP_EPSILON, 1.0 );
- c.bias = approach * (depth - max_penetration) * (1.0/p_step);
+ approach = CLAMP(approach, CMP_EPSILON, 1.0);
+ c.bias = approach * (depth - max_penetration) * (1.0 / p_step);
}
#endif
- c.depth=depth;
+ c.depth = depth;
Vector3 j_vec = c.normal * c.acc_normal_impulse + c.acc_tangent_impulse;
- A->apply_impulse( c.rA+A->get_center_of_mass(), -j_vec );
- B->apply_impulse( c.rB+B->get_center_of_mass(), j_vec );
- c.acc_bias_impulse=0;
+ A->apply_impulse(c.rA + A->get_center_of_mass(), -j_vec);
+ B->apply_impulse(c.rB + B->get_center_of_mass(), j_vec);
+ c.acc_bias_impulse = 0;
Vector3 jb_vec = c.normal * c.acc_bias_impulse;
- A->apply_bias_impulse( c.rA+A->get_center_of_mass(), -jb_vec );
- B->apply_bias_impulse( c.rB+B->get_center_of_mass(), jb_vec );
+ A->apply_bias_impulse(c.rA + A->get_center_of_mass(), -jb_vec);
+ B->apply_bias_impulse(c.rB + B->get_center_of_mass(), jb_vec);
- c.bounce = MAX(A->get_bounce(),B->get_bounce());
+ c.bounce = MAX(A->get_bounce(), B->get_bounce());
if (c.bounce) {
- Vector3 crA = A->get_angular_velocity().cross( c.rA );
- Vector3 crB = B->get_angular_velocity().cross( c.rB );
+ 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
c.bounce = c.bounce * dv.dot(c.normal);
}
-
-
}
return true;
@@ -392,68 +368,63 @@ void BodyPairSW::solve(real_t p_step) {
if (!collided)
return;
-
- for(int i=0;i<contact_count;i++) {
+ 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
+ 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 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) {
+ if (Math::abs(-vbn + c.bias) > MIN_VELOCITY) {
- real_t jbn = (-vbn + c.bias)*c.mass_normal;
+ 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 + A->get_center_of_mass(), -jb);
+ B->apply_bias_impulse(c.rB + B->get_center_of_mass(), jb);
- A->apply_bias_impulse(c.rA+A->get_center_of_mass(),-jb);
- B->apply_bias_impulse(c.rB+B->get_center_of_mass(), jb);
-
- c.active=true;
+ c.active = true;
}
-
- Vector3 crA = A->get_angular_velocity().cross( c.rA );
- Vector3 crB = B->get_angular_velocity().cross( c.rB );
+ 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) {
+ if (Math::abs(vn) > MIN_VELOCITY) {
- real_t jn = -(c.bounce + vn)*c.mass_normal;
+ real_t jn = -(c.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);
- Vector3 j =c.normal * (c.acc_normal_impulse - jnOld);
-
-
- A->apply_impulse(c.rA+A->get_center_of_mass(),-j);
- B->apply_impulse(c.rB+B->get_center_of_mass(), j);
+ A->apply_impulse(c.rA + A->get_center_of_mass(), -j);
+ B->apply_impulse(c.rB + B->get_center_of_mass(), j);
- c.active=true;
+ 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 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);
@@ -466,15 +437,14 @@ void BodyPairSW::solve(real_t p_step) {
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 ) );
+ 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)));
+ (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;
@@ -483,46 +453,35 @@ void BodyPairSW::solve(real_t p_step) {
if (fi_len > CMP_EPSILON && fi_len > jtMax) {
- c.acc_tangent_impulse*=jtMax / fi_len;
+ c.acc_tangent_impulse *= jtMax / fi_len;
}
jt = c.acc_tangent_impulse - jtOld;
+ A->apply_impulse(c.rA + A->get_center_of_mass(), -jt);
+ B->apply_impulse(c.rB + B->get_center_of_mass(), jt);
- A->apply_impulse( c.rA+A->get_center_of_mass(), -jt );
- B->apply_impulse( c.rB+B->get_center_of_mass(), jt );
-
- c.active=true;
-
+ 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(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);
-
}
generated by cgit v1.2.3 (git 2.25.1) at 2025-02-16 16:45:36 +0000