HingeJointBullet::HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameA, const Transform &frameB) :
JointBullet() {
Transform scaled_AFrame(frameA.scaled(rbA->get_body_scale()));
scaled_AFrame.basis.rotref_posscale_decomposition(scaled_AFrame.basis);
btTransform btFrameA;
G_TO_B(scaled_AFrame, btFrameA);
if (rbB) {
Transform scaled_BFrame(frameB.scaled(rbB->get_body_scale()));
scaled_BFrame.basis.rotref_posscale_decomposition(scaled_BFrame.basis);
btTransform btFrameB;
G_TO_B(scaled_BFrame, btFrameB);
hingeConstraint = bulletnew(btHingeConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB));
} else {
hingeConstraint = bulletnew(btHingeConstraint(*rbA->get_bt_rigid_body(), btFrameA));
}
setup(hingeConstraint);
}
Generic6DOFJointBullet::Generic6DOFJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameInA, const Transform &frameInB, bool useLinearReferenceFrameA) :
JointBullet() {
Transform scaled_AFrame(frameInA.scaled(rbA->get_body_scale()));
scaled_AFrame.basis.rotref_posscale_decomposition(scaled_AFrame.basis);
btTransform btFrameA;
G_TO_B(scaled_AFrame, btFrameA);
if (rbB) {
Transform scaled_BFrame(frameInB.scaled(rbB->get_body_scale()));
scaled_BFrame.basis.rotref_posscale_decomposition(scaled_BFrame.basis);
btTransform btFrameB;
G_TO_B(scaled_BFrame, btFrameB);
sixDOFConstraint = bulletnew(btGeneric6DofConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB, useLinearReferenceFrameA));
} else {
sixDOFConstraint = bulletnew(btGeneric6DofConstraint(*rbA->get_bt_rigid_body(), btFrameA, useLinearReferenceFrameA));
}
setup(sixDOFConstraint);
}
void RigidBodyBullet::apply_force(const Vector3 &p_force, const Vector3 &p_pos) {
btVector3 btForce;
btVector3 btPos;
G_TO_B(p_force, btForce);
G_TO_B(p_pos, btPos);
if (Vector3() != p_force)
btBody->activate();
btBody->applyForce(btForce, btPos);
}
void RigidBodyBullet::apply_impulse(const Vector3 &p_pos, const Vector3 &p_impulse) {
btVector3 btImpu;
btVector3 btPos;
G_TO_B(p_impulse, btImpu);
G_TO_B(p_pos, btPos);
if (Vector3() != p_impulse)
btBody->activate();
btBody->applyImpulse(btImpu, btPos);
}
SliderJointBullet::SliderJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameInA, const Transform &frameInB)
: JointBullet() {
btTransform btFrameA;
G_TO_B(frameInA, btFrameA);
if (rbB) {
btTransform btFrameB;
G_TO_B(frameInB, btFrameB);
sliderConstraint = bulletnew(btSliderConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB, true));
} else {
sliderConstraint = bulletnew(btSliderConstraint(*rbA->get_bt_rigid_body(), btFrameA, true));
}
setup(sliderConstraint);
}
void RigidBodyBullet::apply_central_impulse(const Vector3 &p_impulse) {
btVector3 btImpu;
G_TO_B(p_impulse, btImpu);
if (Vector3() != p_impulse)
btBody->activate();
btBody->applyCentralImpulse(btImpu);
}
void RigidBodyBullet::apply_torque(const Vector3 &p_torque) {
btVector3 btTorq;
G_TO_B(p_torque, btTorq);
if (Vector3() != p_torque)
btBody->activate();
btBody->applyTorque(btTorq);
}
void RigidBodyBullet::set_angular_velocity(const Vector3 &p_velocity) {
btVector3 btVec;
G_TO_B(p_velocity, btVec);
if (Vector3() != p_velocity)
btBody->activate();
btBody->setAngularVelocity(btVec);
}
void SoftBodyBullet::move_all_nodes(const Transform &p_transform) {
if (!bt_soft_body)
return;
btTransform bt_transf;
G_TO_B(p_transform, bt_transf);
bt_soft_body->transform(bt_transf);
}
void SoftBodyBullet::reload_soft_body() {
destroy_soft_body();
create_soft_body();
if (bt_soft_body) {
// TODO the softbody set new transform considering the current transform as center of world
// like if it's local transform, so I must fix this by setting nwe transform considering the old
btTransform bt_trans;
G_TO_B(transform, bt_trans);
bt_soft_body->transform(bt_trans);
bt_soft_body->generateBendingConstraints(2, mat0);
mat0->m_kAST = stiffness;
mat0->m_kLST = stiffness;
mat0->m_kVST = stiffness;
bt_soft_body->m_cfg.piterations = simulation_precision;
bt_soft_body->m_cfg.kDP = damping_coefficient;
bt_soft_body->m_cfg.kDG = drag_coefficient;
bt_soft_body->m_cfg.kPR = pressure_coefficient;
bt_soft_body->setTotalMass(mass);
}
if (space) {
// TODO remove this please
space->add_soft_body(this);
}
}
void RigidBodyBullet::apply_central_force(const Vector3 &p_force) {
btVector3 btForce;
G_TO_B(p_force, btForce);
if (Vector3() != p_force)
btBody->activate();
btBody->applyCentralForce(btForce);
}
void RigidBodyBullet::apply_torque_impulse(const Vector3 &p_impulse) {
btVector3 btImp;
G_TO_B(p_impulse, btImp);
if (Vector3() != p_impulse)
btBody->activate();
btBody->applyTorqueImpulse(btImp);
}
void CollisionObjectBullet::set_transform(const Transform &p_global_transform) {
set_body_scale(p_global_transform.basis.get_scale_abs());
btTransform bt_transform;
G_TO_B(p_global_transform, bt_transform);
UNSCALE_BT_BASIS(bt_transform);
set_transform__bullet(bt_transform);
}
void SoftBodyBullet::set_transform(const Transform &p_transform) {
transform = p_transform;
if (bt_soft_body) {
// TODO the softbody set new transform considering the current transform as center of world
// like if it's local transform, so I must fix this by setting nwe transform considering the old
btTransform bt_trans;
G_TO_B(transform, bt_trans);
//bt_soft_body->transform(bt_trans);
}
}
Vector3 BulletPhysicsDirectBodyState::get_contact_collider_velocity_at_position(int p_contact_idx) const {
RigidBodyBullet::CollisionData &colDat = body->collisions.write[p_contact_idx];
btVector3 hitLocation;
G_TO_B(colDat.hitLocalLocation, hitLocation);
Vector3 velocityAtPoint;
B_TO_G(colDat.otherObject->get_bt_rigid_body()->getVelocityInLocalPoint(hitLocation), velocityAtPoint);
return velocityAtPoint;
}
void RigidBodyBullet::set_applied_torque(const Vector3 &p_torque) {
btVector3 btVec = btBody->getTotalForce();
if (Vector3() != p_torque)
btBody->activate();
btBody->clearForces();
btBody->applyCentralForce(btVec);
G_TO_B(p_torque, btVec);
btBody->applyTorque(btVec);
}
HingeJointBullet::HingeJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB) :
JointBullet() {
btVector3 btPivotA;
btVector3 btAxisA;
G_TO_B(pivotInA * rbA->get_body_scale(), btPivotA);
G_TO_B(axisInA * rbA->get_body_scale(), btAxisA);
if (rbB) {
btVector3 btPivotB;
btVector3 btAxisB;
G_TO_B(pivotInB * rbB->get_body_scale(), btPivotB);
G_TO_B(axisInB * rbB->get_body_scale(), btAxisB);
hingeConstraint = bulletnew(btHingeConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btPivotA, btPivotB, btAxisA, btAxisB));
} else {
hingeConstraint = bulletnew(btHingeConstraint(*rbA->get_bt_rigid_body(), btPivotA, btAxisA));
}
setup(hingeConstraint);
}
SliderJointBullet::SliderJointBullet(RigidBodyBullet *rbA, RigidBodyBullet *rbB, const Transform &frameInA, const Transform &frameInB) :
JointBullet() {
Transform scaled_AFrame(frameInA.scaled(rbA->get_body_scale()));
scaled_AFrame.basis.rotref_posscale_decomposition(scaled_AFrame.basis);
btTransform btFrameA;
G_TO_B(scaled_AFrame, btFrameA);
if (rbB) {
Transform scaled_BFrame(frameInB.scaled(rbB->get_body_scale()));
scaled_BFrame.basis.rotref_posscale_decomposition(scaled_BFrame.basis);
btTransform btFrameB;
G_TO_B(scaled_BFrame, btFrameB);
sliderConstraint = bulletnew(btSliderConstraint(*rbA->get_bt_rigid_body(), *rbB->get_bt_rigid_body(), btFrameA, btFrameB, true));
} else {
sliderConstraint = bulletnew(btSliderConstraint(*rbA->get_bt_rigid_body(), btFrameA, true));
}
setup(sliderConstraint);
}
void SoftBodyBullet::reset_all_node_positions() {
if (soft_mesh.is_null())
return;
Array arrays = soft_mesh->surface_get_arrays(0);
PoolVector<Vector3> vs_vertices(arrays[VS::ARRAY_VERTEX]);
PoolVector<Vector3>::Read vs_vertices_read = vs_vertices.read();
for (int vertex_index = bt_soft_body->m_nodes.size() - 1; 0 <= vertex_index; --vertex_index) {
G_TO_B(vs_vertices_read[indices_table[vertex_index][0]], bt_soft_body->m_nodes[vertex_index].m_x);
bt_soft_body->m_nodes[vertex_index].m_q = bt_soft_body->m_nodes[vertex_index].m_x;
bt_soft_body->m_nodes[vertex_index].m_v = btVector3(0, 0, 0);
bt_soft_body->m_nodes[vertex_index].m_f = btVector3(0, 0, 0);
}
}
void G_TO_B(Basis const &inVal, btMatrix3x3 &outVal) {
G_TO_B(inVal[0], outVal[0]);
G_TO_B(inVal[1], outVal[1]);
G_TO_B(inVal[2], outVal[2]);
}
void RigidBodyBullet::reload_space_override_modificator() {
// Make sure that kinematic bodies have their total gravity calculated
if (!is_active() && PhysicsServer::BODY_MODE_KINEMATIC != mode)
return;
Vector3 newGravity(space->get_gravity_direction() * space->get_gravity_magnitude());
real_t newLinearDamp(linearDamp);
real_t newAngularDamp(angularDamp);
AreaBullet *currentArea;
// Variable used to calculate new gravity for gravity point areas, it is pointed by currentGravity pointer
Vector3 support_gravity(0, 0, 0);
int countCombined(0);
for (int i = areaWhereIamCount - 1; 0 <= i; --i) {
currentArea = areasWhereIam[i];
if (PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED == currentArea->get_spOv_mode()) {
continue;
}
/// Here is calculated the gravity
if (currentArea->is_spOv_gravityPoint()) {
/// It calculates the direction of new gravity
support_gravity = currentArea->get_transform().xform(currentArea->get_spOv_gravityVec()) - get_transform().get_origin();
real_t distanceMag = support_gravity.length();
// Normalized in this way to avoid the double call of function "length()"
if (distanceMag == 0) {
support_gravity.x = 0;
support_gravity.y = 0;
support_gravity.z = 0;
} else {
support_gravity.x /= distanceMag;
support_gravity.y /= distanceMag;
support_gravity.z /= distanceMag;
}
/// Here is calculated the final gravity
if (currentArea->get_spOv_gravityPointDistanceScale() > 0) {
// Scaled gravity by distance
support_gravity *= currentArea->get_spOv_gravityMag() / Math::pow(distanceMag * currentArea->get_spOv_gravityPointDistanceScale() + 1, 2);
} else {
// Unscaled gravity
support_gravity *= currentArea->get_spOv_gravityMag();
}
} else {
support_gravity = currentArea->get_spOv_gravityVec() * currentArea->get_spOv_gravityMag();
}
switch (currentArea->get_spOv_mode()) {
///case PhysicsServer::AREA_SPACE_OVERRIDE_DISABLED:
/// This area does not affect gravity/damp. These are generally areas
/// that exist only to detect collisions, and objects entering or exiting them.
/// break;
case PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE:
/// This area adds its gravity/damp values to whatever has been
/// calculated so far. This way, many overlapping areas can combine
/// their physics to make interesting
newGravity += support_gravity;
newLinearDamp += currentArea->get_spOv_linearDamp();
newAngularDamp += currentArea->get_spOv_angularDamp();
++countCombined;
break;
case PhysicsServer::AREA_SPACE_OVERRIDE_COMBINE_REPLACE:
/// This area adds its gravity/damp values to whatever has been calculated
/// so far. Then stops taking into account the rest of the areas, even the
/// default one.
newGravity += support_gravity;
newLinearDamp += currentArea->get_spOv_linearDamp();
newAngularDamp += currentArea->get_spOv_angularDamp();
++countCombined;
goto endAreasCycle;
case PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE:
/// This area replaces any gravity/damp, even the default one, and
/// stops taking into account the rest of the areas.
newGravity = support_gravity;
newLinearDamp = currentArea->get_spOv_linearDamp();
newAngularDamp = currentArea->get_spOv_angularDamp();
countCombined = 1;
goto endAreasCycle;
case PhysicsServer::AREA_SPACE_OVERRIDE_REPLACE_COMBINE:
/// This area replaces any gravity/damp calculated so far, but keeps
/// calculating the rest of the areas, down to the default one.
newGravity = support_gravity;
newLinearDamp = currentArea->get_spOv_linearDamp();
newAngularDamp = currentArea->get_spOv_angularDamp();
countCombined = 1;
break;
}
}
endAreasCycle:
if (1 < countCombined) {
newGravity /= countCombined;
newLinearDamp /= countCombined;
newAngularDamp /= countCombined;
}
btVector3 newBtGravity;
G_TO_B(newGravity * gravity_scale, newBtGravity);
btBody->setGravity(newBtGravity);
btBody->setDamping(newLinearDamp, newAngularDamp);
}
void SoftBodyBullet::set_node_position(int p_node_index, const Vector3 &p_global_position) {
btVector3 bt_pos;
G_TO_B(p_global_position, bt_pos);
set_node_position(p_node_index, bt_pos);
}
void SoftBodyBullet::get_node_offset(int p_node_index, btVector3 &r_offset) const {
Vector3 off;
get_node_offset(p_node_index, off);
G_TO_B(off, r_offset);
}
btVector3 CollisionObjectBullet::get_bt_body_scale() const {
btVector3 s;
G_TO_B(body_scale, s);
return s;
}
void Generic6DOFJointBullet::set_angular_upper_limit(const Vector3 &angularUpper) {
btVector3 btVec;
G_TO_B(angularUpper, btVec);
sixDOFConstraint->setAngularUpperLimit(btVec);
}
void CollisionObjectBullet::ShapeWrapper::set_transform(const Transform &p_transform) {
G_TO_B(p_transform.get_basis().get_scale_abs(), scale);
G_TO_B(p_transform, transform);
UNSCALE_BT_BASIS(transform);
}
void Generic6DOFJointBullet::set_linear_lower_limit(const Vector3 &linearLower) {
btVector3 btVec;
G_TO_B(linearLower, btVec);
sixDOFConstraint->setLinearLowerLimit(btVec);
}
void G_TO_B(Transform const &inVal, btTransform &outVal) {
G_TO_B(inVal.basis, outVal.getBasis());
G_TO_B(inVal.origin, outVal.getOrigin());
}