btScalar GodotRestInfoContactResultCallback::addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1) {
if (cp.getDistance() <= m_min_distance) {
m_min_distance = cp.getDistance();
CollisionObjectBullet *colObj;
if (m_self_object == colObj0Wrap->getCollisionObject()) {
colObj = static_cast<CollisionObjectBullet *>(colObj1Wrap->getCollisionObject()->getUserPointer());
m_result->shape = cp.m_index1;
B_TO_G(cp.getPositionWorldOnB(), m_result->point);
m_rest_info_bt_point = cp.getPositionWorldOnB();
m_rest_info_collision_object = colObj1Wrap->getCollisionObject();
} else {
colObj = static_cast<CollisionObjectBullet *>(colObj0Wrap->getCollisionObject()->getUserPointer());
m_result->shape = cp.m_index0;
B_TO_G(cp.m_normalWorldOnB * -1, m_result->normal);
m_rest_info_bt_point = cp.getPositionWorldOnA();
m_rest_info_collision_object = colObj0Wrap->getCollisionObject();
}
if (colObj)
m_result->collider_id = colObj->get_instance_id();
else
m_result->collider_id = 0;
m_result->rid = colObj->get_self();
m_collided = true;
}
return cp.getDistance();
}
btScalar GodotContactPairContactResultCallback::addSingleResult(btManifoldPoint &cp, const btCollisionObjectWrapper *colObj0Wrap, int partId0, int index0, const btCollisionObjectWrapper *colObj1Wrap, int partId1, int index1) {
if (m_self_object == colObj0Wrap->getCollisionObject()) {
B_TO_G(cp.m_localPointA, m_results[m_count * 2 + 0]); // Local contact
B_TO_G(cp.m_localPointB, m_results[m_count * 2 + 1]);
} else {
B_TO_G(cp.m_localPointB, m_results[m_count * 2 + 0]); // Local contact
B_TO_G(cp.m_localPointA, m_results[m_count * 2 + 1]);
}
++m_count;
return m_count < m_resultMax;
}
void SoftBodyBullet::dispatch_callbacks() {
if (!bt_soft_body) {
return;
}
if (!test_is_in_scene) {
test_is_in_scene = true;
SceneTree::get_singleton()->get_current_scene()->add_child(test_geometry);
}
test_geometry->clear();
test_geometry->begin(Mesh::PRIMITIVE_LINES, NULL);
bool first = true;
Vector3 pos;
for (int i = 0; i < bt_soft_body->m_nodes.size(); ++i) {
const btSoftBody::Node &n = bt_soft_body->m_nodes[i];
B_TO_G(n.m_x, pos);
test_geometry->add_vertex(pos);
if (!first) {
test_geometry->add_vertex(pos);
} else {
first = false;
}
}
test_geometry->end();
}
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 SoftBodyBullet::update_visual_server(SoftBodyVisualServerHandler *p_visual_server_handler) {
if (!bt_soft_body)
return;
/// Update visual server vertices
const btSoftBody::tNodeArray &nodes(bt_soft_body->m_nodes);
const int nodes_count = nodes.size();
const Vector<int> *vs_indices;
const void *vertex_position;
const void *vertex_normal;
for (int vertex_index = 0; vertex_index < nodes_count; ++vertex_index) {
vertex_position = reinterpret_cast<const void *>(&nodes[vertex_index].m_x);
vertex_normal = reinterpret_cast<const void *>(&nodes[vertex_index].m_n);
vs_indices = &indices_table[vertex_index];
const int vs_indices_size(vs_indices->size());
for (int x = 0; x < vs_indices_size; ++x) {
p_visual_server_handler->set_vertex((*vs_indices)[x], vertex_position);
p_visual_server_handler->set_normal((*vs_indices)[x], vertex_normal);
}
}
/// Generate AABB
btVector3 aabb_min;
btVector3 aabb_max;
bt_soft_body->getAabb(aabb_min, aabb_max);
btVector3 size(aabb_max - aabb_min);
AABB aabb;
B_TO_G(aabb_min, aabb.position);
B_TO_G(size, aabb.size);
p_visual_server_handler->set_aabb(aabb);
}
Vector3 RigidBodyBullet::get_applied_torque() const {
Vector3 gTotTorq;
B_TO_G(btBody->getTotalTorque(), gTotTorq);
return gTotTorq;
}
Vector3 RigidBodyBullet::get_applied_force() const {
Vector3 gTotForc;
B_TO_G(btBody->getTotalForce(), gTotForc);
return gTotForc;
}
Vector3 BulletPhysicsDirectBodyState::get_center_of_mass() const {
Vector3 gVec;
B_TO_G(body->btBody->getCenterOfMassPosition(), gVec);
return gVec;
}
Vector3 BulletPhysicsDirectBodyState::get_total_gravity() const {
Vector3 gVec;
B_TO_G(body->btBody->getGravity(), gVec);
return gVec;
}
const Transform SliderJointBullet::getFrameOffsetB() const {
btTransform btTransform = sliderConstraint->getFrameOffsetB();
Transform gTrans;
B_TO_G(btTransform, gTrans);
return gTrans;
}
Basis BulletPhysicsDirectBodyState::get_inverse_inertia_tensor() const {
Basis gInertia;
B_TO_G(body->btBody->getInvInertiaTensorWorld(), gInertia);
return gInertia;
}
void B_TO_G(btTransform const &inVal, Transform &outVal) {
B_TO_G(inVal.getBasis(), outVal.basis);
B_TO_G(inVal.getOrigin(), outVal.origin);
}
void B_TO_G(btMatrix3x3 const &inVal, Basis &outVal) {
B_TO_G(inVal[0], outVal[0]);
B_TO_G(inVal[1], outVal[1]);
B_TO_G(inVal[2], outVal[2]);
}
Transform RigidCollisionObjectBullet::get_shape_transform(int p_index) const {
Transform trs;
B_TO_G(shapes[p_index].transform, trs);
return trs;
}
Transform CollisionObjectBullet::get_transform() const {
Transform t;
B_TO_G(get_transform__bullet(), t);
t.basis.scale(body_scale);
return t;
}
Vector3 RigidBodyBullet::get_angular_velocity() const {
Vector3 gVec;
B_TO_G(btBody->getAngularVelocity(), gVec);
return gVec;
}
Vector3 BulletPhysicsDirectBodyState::get_inverse_inertia() const {
Vector3 gVec;
B_TO_G(body->btBody->getInvInertiaDiagLocal(), gVec);
return gVec;
}
void SoftBodyBullet::get_node_position(int p_node_index, Vector3 &r_position) const {
if (bt_soft_body) {
B_TO_G(bt_soft_body->m_nodes[p_node_index].m_x, r_position);
}
}
Transform Generic6DOFJointBullet::getFrameOffsetB() {
btTransform btTrs = sixDOFConstraint->getFrameOffsetB();
Transform gTrs;
B_TO_G(btTrs, gTrs);
return gTrs;
}
const Transform SliderJointBullet::getCalculatedTransformB() const {
btTransform btTransform = sliderConstraint->getCalculatedTransformB();
Transform gTrans;
B_TO_G(btTransform, gTrans);
return gTrans;
}
