bool KinematicBody::can_teleport_to(const Vector3 &p_position) {
ERR_FAIL_COND_V(!is_inside_tree(), false);
PhysicsDirectSpaceState *dss = PhysicsServer::get_singleton()->space_get_direct_state(get_world()->get_space());
ERR_FAIL_COND_V(!dss, false);
uint32_t mask = 0;
if (collide_static)
mask |= PhysicsDirectSpaceState::TYPE_MASK_STATIC_BODY;
if (collide_kinematic)
mask |= PhysicsDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
if (collide_rigid)
mask |= PhysicsDirectSpaceState::TYPE_MASK_RIGID_BODY;
if (collide_character)
mask |= PhysicsDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
Transform xform = get_global_transform();
xform.origin = p_position;
Set<RID> exclude;
exclude.insert(get_rid());
for (int i = 0; i < get_shape_count(); i++) {
if (is_shape_set_as_trigger(i))
continue;
bool col = dss->intersect_shape(get_shape(i)->get_rid(), xform * get_shape_transform(i), 0, NULL, 1, exclude, get_collision_layer(), mask);
if (col)
return false;
}
return true;
}
void RayCast::_notification(int p_what) {
switch(p_what) {
case NOTIFICATION_ENTER_TREE: {
if (enabled && !get_tree()->is_editor_hint()) {
set_fixed_process(true);
} else
set_fixed_process(false);
} break;
case NOTIFICATION_EXIT_TREE: {
if (enabled) {
set_fixed_process(false);
}
} break;
case NOTIFICATION_FIXED_PROCESS: {
if (!enabled)
break;
Ref<World> w3d = get_world();
ERR_BREAK( w3d.is_null() );
PhysicsDirectSpaceState *dss = PhysicsServer::get_singleton()->space_get_direct_state(w3d->get_space());
ERR_BREAK( !dss );
Transform gt = get_global_transform();
Vector3 to = cast_to;
if (to==Vector3())
to=Vector3(0,0.01,0);
PhysicsDirectSpaceState::RayResult rr;
if (dss->intersect_ray(gt.get_origin(),gt.xform(to),rr,exclude)) {
collided=true;
against=rr.collider_id;
collision_point=rr.position;
collision_normal=rr.normal;
against_shape=rr.shape;
} else {
collided=false;
}
} break;
}
}
bool KinematicBody::can_move_to(const Vector3& p_position, bool p_discrete) {
ERR_FAIL_COND_V(!is_inside_scene(),false);
PhysicsDirectSpaceState *dss = PhysicsServer::get_singleton()->space_get_direct_state(get_world()->get_space());
ERR_FAIL_COND_V(!dss,false);
uint32_t mask=0;
if (collide_static)
mask|=PhysicsDirectSpaceState::TYPE_MASK_STATIC_BODY;
if (collide_kinematic)
mask|=PhysicsDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
if (collide_rigid)
mask|=PhysicsDirectSpaceState::TYPE_MASK_RIGID_BODY;
if (collide_character)
mask|=PhysicsDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
Vector3 motion = p_position-get_global_transform().origin;
Transform xform=get_global_transform();
if (true || p_discrete) {
xform.origin+=motion;
motion=Vector3();
}
Set<RID> exclude;
exclude.insert(get_rid());
//fill exclude list..
for(int i=0;i<get_shape_count();i++) {
bool col = dss->intersect_shape(get_shape(i)->get_rid(), xform * get_shape_transform(i),0,NULL,0,exclude,get_layer_mask(),mask);
if (col)
return false;
}
return true;
}
Vector3 KinematicBody::move(const Vector3& p_motion) {
//give me back regular physics engine logic
//this is madness
//and most people using this function will think
//what it does is simpler than using physics
//this took about a week to get right..
//but is it right? who knows at this point..
colliding=false;
ERR_FAIL_COND_V(!is_inside_scene(),Vector3());
PhysicsDirectSpaceState *dss = PhysicsServer::get_singleton()->space_get_direct_state(get_world()->get_space());
ERR_FAIL_COND_V(!dss,Vector3());
const int max_shapes=32;
Vector3 sr[max_shapes*2];
int res_shapes;
Set<RID> exclude;
exclude.insert(get_rid());
//recover first
int recover_attempts=4;
bool collided=false;
uint32_t mask=0;
if (collide_static)
mask|=PhysicsDirectSpaceState::TYPE_MASK_STATIC_BODY;
if (collide_kinematic)
mask|=PhysicsDirectSpaceState::TYPE_MASK_KINEMATIC_BODY;
if (collide_rigid)
mask|=PhysicsDirectSpaceState::TYPE_MASK_RIGID_BODY;
if (collide_character)
mask|=PhysicsDirectSpaceState::TYPE_MASK_CHARACTER_BODY;
// print_line("motion: "+p_motion+" margin: "+rtos(margin));
//print_line("margin: "+rtos(margin));
float m = margin;
//m=0.001;
do {
//motion recover
for(int i=0;i<get_shape_count();i++) {
if (dss->collide_shape(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i),m,sr,max_shapes,res_shapes,exclude,get_layer_mask(),mask)) {
collided=true;
}
}
if (!collided)
break;
//print_line("have to recover");
Vector3 recover_motion;
bool all_outside=true;
for(int j=0;j<8;j++) {
for(int i=0;i<res_shapes;i++) {
Vector3 a = sr[i*2+0];
Vector3 b = sr[i*2+1];
//print_line(String()+a+" -> "+b);
#if 0
float d = a.distance_to(b);
//if (d<margin)
/// continue;
///
///
recover_motion+=(b-a)*0.2;
#else
float dist = a.distance_to(b);
if (dist>CMP_EPSILON) {
Vector3 norm = (b-a).normalized();
if (dist>margin*0.5)
all_outside=false;
float adv = norm.dot(recover_motion);
//print_line(itos(i)+" dist: "+rtos(dist)+" adv: "+rtos(adv));
recover_motion+=norm*MAX(dist-adv,0)*0.4;
}
#endif
}
}
if (recover_motion==Vector3()) {
collided=false;
break;
}
//print_line("**** RECOVER: "+recover_motion);
Transform gt = get_global_transform();
gt.origin+=recover_motion;
set_global_transform(gt);
recover_attempts--;
if (all_outside)
break;
} while (recover_attempts);
//move second
float safe = 1.0;
float unsafe = 1.0;
int best_shape=-1;
PhysicsDirectSpaceState::ShapeRestInfo rest;
//print_line("pos: "+get_global_transform().origin);
//print_line("motion: "+p_motion);
for(int i=0;i<get_shape_count();i++) {
float lsafe,lunsafe;
PhysicsDirectSpaceState::ShapeRestInfo lrest;
bool valid = dss->cast_motion(get_shape(i)->get_rid(), get_global_transform() * get_shape_transform(i), p_motion,0, lsafe,lunsafe,exclude,get_layer_mask(),mask,&lrest);
//print_line("shape: "+itos(i)+" travel:"+rtos(ltravel));
if (!valid) {
safe=0;
unsafe=0;
best_shape=i; //sadly it's the best
//print_line("initial stuck");
break;
}
if (lsafe==1.0) {
//print_line("initial free");
continue;
}
if (lsafe < safe) {
//print_line("initial at "+rtos(lsafe));
safe=lsafe;
safe=MAX(0,lsafe-0.01);
unsafe=lunsafe;
best_shape=i;
rest=lrest;
}
}
//print_line("best shape: "+itos(best_shape)+" motion "+p_motion);
if (safe>=1) {
//not collided
colliding=false;
} else {
colliding=true;
if (true || (safe==0 && unsafe==0)) { //use it always because it's more precise than GJK
//no advance, use rest info from collision
Transform ugt = get_global_transform();
ugt.origin+=p_motion*unsafe;
PhysicsDirectSpaceState::ShapeRestInfo rest_info;
bool c2 = dss->rest_info(get_shape(best_shape)->get_rid(), ugt*get_shape_transform(best_shape), m,&rest,exclude,get_layer_mask(),mask);
if (!c2) {
//should not happen, but floating point precision is so weird..
colliding=false;
}
// print_line("Rest Travel: "+rest.normal);
}
if (colliding) {
collision=rest.point;
normal=rest.normal;
collider=rest.collider_id;
collider_vel=rest.linear_velocity;
}
}
Vector3 motion=p_motion*safe;
//if (colliding)
// motion+=normal*0.001;
Transform gt = get_global_transform();
gt.origin+=motion;
set_global_transform(gt);
return p_motion-motion;
}
real_t VehicleBody::_ray_cast(int p_idx, PhysicsDirectBodyState *s) {
VehicleWheel &wheel = *wheels[p_idx];
_update_wheel_transform(wheel, s);
real_t depth = -1;
real_t raylen = wheel.m_suspensionRestLength + wheel.m_wheelRadius;
Vector3 rayvector = wheel.m_raycastInfo.m_wheelDirectionWS * (raylen);
Vector3 source = wheel.m_raycastInfo.m_hardPointWS;
wheel.m_raycastInfo.m_contactPointWS = source + rayvector;
const Vector3 &target = wheel.m_raycastInfo.m_contactPointWS;
source -= wheel.m_wheelRadius * wheel.m_raycastInfo.m_wheelDirectionWS;
real_t param = real_t(0.);
PhysicsDirectSpaceState::RayResult rr;
PhysicsDirectSpaceState *ss = s->get_space_state();
bool col = ss->intersect_ray(source, target, rr, exclude);
wheel.m_raycastInfo.m_groundObject = 0;
if (col) {
param = source.distance_to(rr.position) / source.distance_to(target);
depth = raylen * param;
wheel.m_raycastInfo.m_contactNormalWS = rr.normal;
wheel.m_raycastInfo.m_isInContact = true;
if (rr.collider)
wheel.m_raycastInfo.m_groundObject = Object::cast_to<PhysicsBody>(rr.collider);
real_t hitDistance = param * raylen;
wheel.m_raycastInfo.m_suspensionLength = hitDistance - wheel.m_wheelRadius;
//clamp on max suspension travel
real_t minSuspensionLength = wheel.m_suspensionRestLength - wheel.m_maxSuspensionTravelCm * real_t(0.01);
real_t maxSuspensionLength = wheel.m_suspensionRestLength + wheel.m_maxSuspensionTravelCm * real_t(0.01);
if (wheel.m_raycastInfo.m_suspensionLength < minSuspensionLength) {
wheel.m_raycastInfo.m_suspensionLength = minSuspensionLength;
}
if (wheel.m_raycastInfo.m_suspensionLength > maxSuspensionLength) {
wheel.m_raycastInfo.m_suspensionLength = maxSuspensionLength;
}
wheel.m_raycastInfo.m_contactPointWS = rr.position;
real_t denominator = wheel.m_raycastInfo.m_contactNormalWS.dot(wheel.m_raycastInfo.m_wheelDirectionWS);
Vector3 chassis_velocity_at_contactPoint;
//Vector3 relpos = wheel.m_raycastInfo.m_contactPointWS-getRigidBody()->getCenterOfMassPosition();
//chassis_velocity_at_contactPoint = getRigidBody()->getVelocityInLocalPoint(relpos);
chassis_velocity_at_contactPoint = s->get_linear_velocity() +
(s->get_angular_velocity()).cross(wheel.m_raycastInfo.m_contactPointWS - s->get_transform().origin); // * mPos);
real_t projVel = wheel.m_raycastInfo.m_contactNormalWS.dot(chassis_velocity_at_contactPoint);
if (denominator >= real_t(-0.1)) {
wheel.m_suspensionRelativeVelocity = real_t(0.0);
wheel.m_clippedInvContactDotSuspension = real_t(1.0) / real_t(0.1);
} else {
real_t inv = real_t(-1.) / denominator;
wheel.m_suspensionRelativeVelocity = projVel * inv;
wheel.m_clippedInvContactDotSuspension = inv;
}
} else {
wheel.m_raycastInfo.m_isInContact = false;
//put wheel info as in rest position
wheel.m_raycastInfo.m_suspensionLength = wheel.m_suspensionRestLength;
wheel.m_suspensionRelativeVelocity = real_t(0.0);
wheel.m_raycastInfo.m_contactNormalWS = -wheel.m_raycastInfo.m_wheelDirectionWS;
wheel.m_clippedInvContactDotSuspension = real_t(1.0);
}
return depth;
}
