void VehicleBody::_update_wheel(int p_idx, PhysicsDirectBodyState *s) { VehicleWheel &wheel = *wheels[p_idx]; _update_wheel_transform(wheel, s); Vector3 up = -wheel.m_raycastInfo.m_wheelDirectionWS; const Vector3 &right = wheel.m_raycastInfo.m_wheelAxleWS; Vector3 fwd = up.cross(right); fwd = fwd.normalized(); //rotate around steering over de wheelAxleWS real_t steering = wheel.steers ? m_steeringValue : 0.0; Basis steeringMat(up, steering); Basis rotatingMat(right, wheel.m_rotation); Basis basis2( right[0], up[0], fwd[0], right[1], up[1], fwd[1], right[2], up[2], fwd[2]); wheel.m_worldTransform.set_basis(steeringMat * rotatingMat * basis2); //wheel.m_worldTransform.set_basis(basis2 * (steeringMat * rotatingMat)); wheel.m_worldTransform.set_origin( wheel.m_raycastInfo.m_hardPointWS + wheel.m_raycastInfo.m_wheelDirectionWS * wheel.m_raycastInfo.m_suspensionLength); }
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) { //print_line("WHEEL "+itos(p_idx)+" FROM "+source+" TO: "+target); //print_line("WHEEL "+itos(p_idx)+" COLLIDE? "+itos(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=rr.collider->cast_to<PhysicsBody>(); 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; }