void StaticBody::_state_notify(Object *p_object) {
if (!pre_xform)
return;
PhysicsDirectBodyState *p2d = (PhysicsDirectBodyState*)p_object;
setting=true;
Transform new_xform = p2d->get_transform();
*pre_xform=new_xform;
set_ignore_transform_notification(true);
set_global_transform(new_xform);
set_ignore_transform_notification(false);
setting=false;
}
void StaticBody::_update_xform() {
if (!pre_xform || !pending)
return;
setting=true;
Transform new_xform = get_global_transform(); //obtain the new one
//set_block_transform_notify(true);
set_ignore_transform_notification(true);
PhysicsServer::get_singleton()->body_set_state(get_rid(),PhysicsServer::BODY_STATE_TRANSFORM,*pre_xform); //then simulate motion!
set_global_transform(*pre_xform); //but restore state to previous one in both visual and physics
set_ignore_transform_notification(false);
PhysicsServer::get_singleton()->body_static_simulate_motion(get_rid(),new_xform); //then simulate motion!
setting=false;
pending=false;
}
void RigidBody::_direct_state_changed(Object *p_state) {
//eh.. f**k
#ifdef DEBUG_ENABLED
state=p_state->cast_to<PhysicsDirectBodyState>();
#else
state=(PhysicsDirectBodyState*)p_state; //trust it
#endif
if (contact_monitor) {
//untag all
int rc=0;
for( Map<ObjectID,BodyState>::Element *E=contact_monitor->body_map.front();E;E=E->next()) {
for(int i=0;i<E->get().shapes.size();i++) {
E->get().shapes[i].tagged=false;
rc++;
}
}
_RigidBodyInOut *toadd=(_RigidBodyInOut*)alloca(state->get_contact_count()*sizeof(_RigidBodyInOut));
int toadd_count=0;//state->get_contact_count();
RigidBody_RemoveAction *toremove=(RigidBody_RemoveAction*)alloca(rc*sizeof(RigidBody_RemoveAction));
int toremove_count=0;
//put the ones to add
for(int i=0;i<state->get_contact_count();i++) {
ObjectID obj = state->get_contact_collider_id(i);
int local_shape = state->get_contact_local_shape(i);
int shape = state->get_contact_collider_shape(i);
toadd[i].local_shape=local_shape;
toadd[i].id=obj;
toadd[i].shape=shape;
bool found=false;
Map<ObjectID,BodyState>::Element *E=contact_monitor->body_map.find(obj);
if (!E) {
toadd_count++;
continue;
}
ShapePair sp( shape,local_shape );
int idx = E->get().shapes.find(sp);
if (idx==-1) {
toadd_count++;
continue;
}
E->get().shapes[idx].tagged=true;
}
//put the ones to remove
for( Map<ObjectID,BodyState>::Element *E=contact_monitor->body_map.front();E;E=E->next()) {
for(int i=0;i<E->get().shapes.size();i++) {
if (!E->get().shapes[i].tagged) {
toremove[toremove_count].body_id=E->key();
toremove[toremove_count].pair=E->get().shapes[i];
toremove_count++;
}
}
}
//process remotions
for(int i=0;i<toremove_count;i++) {
_body_inout(0,toremove[i].body_id,toremove[i].pair.body_shape,toremove[i].pair.local_shape);
}
//process aditions
for(int i=0;i<toadd_count;i++) {
_body_inout(1,toadd[i].id,toadd[i].shape,toadd[i].local_shape);
}
}
set_ignore_transform_notification(true);
set_global_transform(state->get_transform());
linear_velocity=state->get_linear_velocity();
angular_velocity=state->get_angular_velocity();
sleeping=state->is_sleeping();
if (get_script_instance())
get_script_instance()->call("_integrate_forces",state);
set_ignore_transform_notification(false);
state=NULL;
}
void VehicleBody::_direct_state_changed(Object *p_state) {
PhysicsDirectBodyState *s = p_state->cast_to<PhysicsDirectBodyState>();
set_ignore_transform_notification(true);
set_global_transform(s->get_transform());
set_ignore_transform_notification(false);
float step = s->get_step();
for(int i=0;i<wheels.size();i++) {
_update_wheel(i,s);
}
for(int i=0;i<wheels.size();i++) {
_ray_cast(i,s);
wheels[i]->set_transform(s->get_transform().inverse() * wheels[i]->m_worldTransform);
}
_update_suspension(s);
for(int i=0;i<wheels.size();i++) {
//apply suspension force
VehicleWheel& wheel = *wheels[i];
real_t suspensionForce = wheel.m_wheelsSuspensionForce;
if (suspensionForce > wheel.m_maxSuspensionForce)
{
suspensionForce = wheel.m_maxSuspensionForce;
}
Vector3 impulse = wheel.m_raycastInfo.m_contactNormalWS * suspensionForce * step;
Vector3 relpos = wheel.m_raycastInfo.m_contactPointWS - s->get_transform().origin;
s->apply_impulse(relpos,impulse);
//getRigidBody()->applyImpulse(impulse, relpos);
}
_update_friction(s);
for (int i=0;i<wheels.size();i++)
{
VehicleWheel& wheel = *wheels[i];
Vector3 relpos = wheel.m_raycastInfo.m_hardPointWS - s->get_transform().origin;
Vector3 vel = s->get_linear_velocity() + (s->get_angular_velocity()).cross(relpos);// * mPos);
if (wheel.m_raycastInfo.m_isInContact)
{
const Transform& chassisWorldTransform = s->get_transform();
Vector3 fwd (
chassisWorldTransform.basis[0][Vector3::AXIS_Z],
chassisWorldTransform.basis[1][Vector3::AXIS_Z],
chassisWorldTransform.basis[2][Vector3::AXIS_Z]);
real_t proj = fwd.dot(wheel.m_raycastInfo.m_contactNormalWS);
fwd -= wheel.m_raycastInfo.m_contactNormalWS * proj;
real_t proj2 = fwd.dot(vel);
wheel.m_deltaRotation = (proj2 * step) / (wheel.m_wheelRadius);
wheel.m_rotation += wheel.m_deltaRotation;
} else
{
wheel.m_rotation += wheel.m_deltaRotation;
}
wheel.m_deltaRotation *= real_t(0.99);//damping of rotation when not in contact
}
linear_velocity = s->get_linear_velocity();
}
