summaryrefslogtreecommitdiff
path: root/servers/physics_3d/joints
diff options
context:
space:
mode:
authorPouleyKetchoupp <pouleyketchoup@gmail.com>2021-10-18 12:24:30 -0700
committerPouleyKetchoupp <pouleyketchoup@gmail.com>2021-10-18 17:01:10 -0700
commitcc39dca9f7d960d1bb137f1dcbbf1da5cec8a505 (patch)
tree555c5721844576a85183de9f1c33cd19c671f73c /servers/physics_3d/joints
parent5bb3dbbedd4f32974eef36ffc83bbe29abb65ab1 (diff)
Rename Godot Physics classes from *SW to Godot*
Also moved MT physics server wrappers to the main servers folder, since they don't have to be implementation specific.
Diffstat (limited to 'servers/physics_3d/joints')
-rw-r--r--servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp (renamed from servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp)18
-rw-r--r--servers/physics_3d/joints/godot_cone_twist_joint_3d.h (renamed from servers/physics_3d/joints/cone_twist_joint_3d_sw.h)28
-rw-r--r--servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp (renamed from servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp)69
-rw-r--r--servers/physics_3d/joints/godot_generic_6dof_joint_3d.h (renamed from servers/physics_3d/joints/generic_6dof_joint_3d_sw.h)111
-rw-r--r--servers/physics_3d/joints/godot_hinge_joint_3d.cpp (renamed from servers/physics_3d/joints/hinge_joint_3d_sw.cpp)34
-rw-r--r--servers/physics_3d/joints/godot_hinge_joint_3d.h (renamed from servers/physics_3d/joints/hinge_joint_3d_sw.h)28
-rw-r--r--servers/physics_3d/joints/godot_jacobian_entry_3d.h (renamed from servers/physics_3d/joints/jacobian_entry_3d_sw.h)28
-rw-r--r--servers/physics_3d/joints/godot_pin_joint_3d.cpp (renamed from servers/physics_3d/joints/pin_joint_3d_sw.cpp)20
-rw-r--r--servers/physics_3d/joints/godot_pin_joint_3d.h (renamed from servers/physics_3d/joints/pin_joint_3d_sw.h)26
-rw-r--r--servers/physics_3d/joints/godot_slider_joint_3d.cpp (renamed from servers/physics_3d/joints/slider_joint_3d_sw.cpp)48
-rw-r--r--servers/physics_3d/joints/godot_slider_joint_3d.h (renamed from servers/physics_3d/joints/slider_joint_3d_sw.h)30
11 files changed, 206 insertions, 234 deletions
diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp b/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp
index bb9cc1bf67..31a87fc595 100644
--- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/godot_cone_twist_joint_3d.cpp
@@ -1,5 +1,5 @@
/*************************************************************************/
-/* cone_twist_joint_3d_sw.cpp */
+/* godot_cone_twist_joint_3d.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@@ -49,7 +49,7 @@ subject to the following restrictions:
Written by: Marcus Hennix
*/
-#include "cone_twist_joint_3d_sw.h"
+#include "godot_cone_twist_joint_3d.h"
static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) {
if (Math::abs(n.z) > Math_SQRT12) {
@@ -84,8 +84,8 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) {
return (y < 0.0f) ? -angle : angle;
}
-ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame) :
- Joint3DSW(_arr, 2) {
+GodotConeTwistJoint3D::GodotConeTwistJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame) :
+ GodotJoint3D(_arr, 2) {
A = rbA;
B = rbB;
@@ -96,7 +96,7 @@ ConeTwistJoint3DSW::ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Trans
B->add_constraint(this, 1);
}
-bool ConeTwistJoint3DSW::setup(real_t p_timestep) {
+bool GodotConeTwistJoint3D::setup(real_t p_timestep) {
dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
@@ -129,7 +129,7 @@ bool ConeTwistJoint3DSW::setup(real_t p_timestep) {
plane_space(normal[0], normal[1], normal[2]);
for (int i = 0; i < 3; i++) {
- memnew_placement(&m_jac[i], JacobianEntry3DSW(
+ memnew_placement(&m_jac[i], GodotJacobianEntry3D(
A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(),
pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
@@ -230,7 +230,7 @@ bool ConeTwistJoint3DSW::setup(real_t p_timestep) {
return true;
}
-void ConeTwistJoint3DSW::solve(real_t p_timestep) {
+void GodotConeTwistJoint3D::solve(real_t p_timestep) {
Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
@@ -312,7 +312,7 @@ void ConeTwistJoint3DSW::solve(real_t p_timestep) {
}
}
-void ConeTwistJoint3DSW::set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value) {
+void GodotConeTwistJoint3D::set_param(PhysicsServer3D::ConeTwistJointParam p_param, real_t p_value) {
switch (p_param) {
case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: {
m_swingSpan1 = p_value;
@@ -335,7 +335,7 @@ void ConeTwistJoint3DSW::set_param(PhysicsServer3D::ConeTwistJointParam p_param,
}
}
-real_t ConeTwistJoint3DSW::get_param(PhysicsServer3D::ConeTwistJointParam p_param) const {
+real_t GodotConeTwistJoint3D::get_param(PhysicsServer3D::ConeTwistJointParam p_param) const {
switch (p_param) {
case PhysicsServer3D::CONE_TWIST_JOINT_SWING_SPAN: {
return m_swingSpan1;
diff --git a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h b/servers/physics_3d/joints/godot_cone_twist_joint_3d.h
index bf7e593820..999d0f0692 100644
--- a/servers/physics_3d/joints/cone_twist_joint_3d_sw.h
+++ b/servers/physics_3d/joints/godot_cone_twist_joint_3d.h
@@ -1,5 +1,5 @@
/*************************************************************************/
-/* cone_twist_joint_3d_sw.h */
+/* godot_cone_twist_joint_3d.h */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@@ -34,7 +34,7 @@ Adapted to Godot from the Bullet library.
/*
Bullet Continuous Collision Detection and Physics Library
-ConeTwistJointSW is Copyright (c) 2007 Starbreeze Studios
+GodotConeTwistJoint3D is Copyright (c) 2007 Starbreeze Studios
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
@@ -49,28 +49,28 @@ subject to the following restrictions:
Written by: Marcus Hennix
*/
-#ifndef CONE_TWIST_JOINT_SW_H
-#define CONE_TWIST_JOINT_SW_H
+#ifndef GODOT_CONE_TWIST_JOINT_3D_H
+#define GODOT_CONE_TWIST_JOINT_3D_H
-#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h"
-#include "servers/physics_3d/joints_3d_sw.h"
+#include "servers/physics_3d/godot_joint_3d.h"
+#include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
-///ConeTwistJointSW can be used to simulate ragdoll joints (upper arm, leg etc)
-class ConeTwistJoint3DSW : public Joint3DSW {
+// GodotConeTwistJoint3D can be used to simulate ragdoll joints (upper arm, leg etc).
+class GodotConeTwistJoint3D : public GodotJoint3D {
#ifdef IN_PARALLELL_SOLVER
public:
#endif
union {
struct {
- Body3DSW *A;
- Body3DSW *B;
+ GodotBody3D *A;
+ GodotBody3D *B;
};
- Body3DSW *_arr[2] = { nullptr, nullptr };
+ GodotBody3D *_arr[2] = { nullptr, nullptr };
};
- JacobianEntry3DSW m_jac[3] = {}; //3 orthogonal linear constraints
+ GodotJacobianEntry3D m_jac[3] = {}; //3 orthogonal linear constraints
real_t m_appliedImpulse = 0.0;
Transform3D m_rbAFrame;
@@ -107,7 +107,7 @@ public:
virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_step) override;
- ConeTwistJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame);
+ GodotConeTwistJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &rbAFrame, const Transform3D &rbBFrame);
void setAngularOnly(bool angularOnly) {
m_angularOnly = angularOnly;
@@ -139,4 +139,4 @@ public:
real_t get_param(PhysicsServer3D::ConeTwistJointParam p_param) const;
};
-#endif // CONE_TWIST_JOINT_SW_H
+#endif // GODOT_CONE_TWIST_JOINT_3D_H
diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp
index 56aba24b42..d7e0537439 100644
--- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.cpp
@@ -1,5 +1,5 @@
/*************************************************************************/
-/* generic_6dof_joint_3d_sw.cpp */
+/* godot_generic_6dof_joint_3d.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@@ -49,18 +49,18 @@ subject to the following restrictions:
/*
2007-09-09
-Generic6DOFJointSW Refactored by Francisco Le?n
+GodotGeneric6DOFJoint3D Refactored by Francisco Le?n
email: projectileman@yahoo.com
http://gimpact.sf.net
*/
-#include "generic_6dof_joint_3d_sw.h"
+#include "godot_generic_6dof_joint_3d.h"
#define GENERIC_D6_DISABLE_WARMSTARTING 1
-//////////////////////////// G6DOFRotationalLimitMotorSW ////////////////////////////////////
+//////////////////////////// GodotG6DOFRotationalLimitMotor3D ////////////////////////////////////
-int G6DOFRotationalLimitMotor3DSW::testLimitValue(real_t test_value) {
+int GodotG6DOFRotationalLimitMotor3D::testLimitValue(real_t test_value) {
if (m_loLimit > m_hiLimit) {
m_currentLimit = 0; //Free from violation
return 0;
@@ -80,9 +80,9 @@ int G6DOFRotationalLimitMotor3DSW::testLimitValue(real_t test_value) {
return 0;
}
-real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits(
+real_t GodotG6DOFRotationalLimitMotor3D::solveAngularLimits(
real_t timeStep, Vector3 &axis, real_t jacDiagABInv,
- Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic) {
+ GodotBody3D *body0, GodotBody3D *body1, bool p_body0_dynamic, bool p_body1_dynamic) {
if (!needApplyTorques()) {
return 0.0f;
}
@@ -148,14 +148,13 @@ real_t G6DOFRotationalLimitMotor3DSW::solveAngularLimits(
return clippedMotorImpulse;
}
-//////////////////////////// End G6DOFRotationalLimitMotorSW ////////////////////////////////////
+//////////////////////////// GodotG6DOFTranslationalLimitMotor3D ////////////////////////////////////
-//////////////////////////// G6DOFTranslationalLimitMotorSW ////////////////////////////////////
-real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis(
+real_t GodotG6DOFTranslationalLimitMotor3D::solveLinearAxis(
real_t timeStep,
real_t jacDiagABInv,
- Body3DSW *body1, const Vector3 &pointInA,
- Body3DSW *body2, const Vector3 &pointInB,
+ GodotBody3D *body1, const Vector3 &pointInA,
+ GodotBody3D *body2, const Vector3 &pointInB,
bool p_body1_dynamic, bool p_body2_dynamic,
int limit_index,
const Vector3 &axis_normal_on_a,
@@ -217,10 +216,10 @@ real_t G6DOFTranslationalLimitMotor3DSW::solveLinearAxis(
return normalImpulse;
}
-//////////////////////////// G6DOFTranslationalLimitMotorSW ////////////////////////////////////
+//////////////////////////// GodotGeneric6DOFJoint3D ////////////////////////////////////
-Generic6DOFJoint3DSW::Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA) :
- Joint3DSW(_arr, 2),
+GodotGeneric6DOFJoint3D::GodotGeneric6DOFJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA) :
+ GodotJoint3D(_arr, 2),
m_frameInA(frameInA),
m_frameInB(frameInB),
m_useLinearReferenceFrameA(useLinearReferenceFrameA) {
@@ -230,7 +229,7 @@ Generic6DOFJoint3DSW::Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const T
B->add_constraint(this, 1);
}
-void Generic6DOFJoint3DSW::calculateAngleInfo() {
+void GodotGeneric6DOFJoint3D::calculateAngleInfo() {
Basis relative_frame = m_calculatedTransformB.basis.inverse() * m_calculatedTransformA.basis;
m_calculatedAxisAngleDiff = relative_frame.get_euler_xyz();
@@ -270,17 +269,17 @@ void Generic6DOFJoint3DSW::calculateAngleInfo() {
*/
}
-void Generic6DOFJoint3DSW::calculateTransforms() {
+void GodotGeneric6DOFJoint3D::calculateTransforms() {
m_calculatedTransformA = A->get_transform() * m_frameInA;
m_calculatedTransformB = B->get_transform() * m_frameInB;
calculateAngleInfo();
}
-void Generic6DOFJoint3DSW::buildLinearJacobian(
- JacobianEntry3DSW &jacLinear, const Vector3 &normalWorld,
+void GodotGeneric6DOFJoint3D::buildLinearJacobian(
+ GodotJacobianEntry3D &jacLinear, const Vector3 &normalWorld,
const Vector3 &pivotAInW, const Vector3 &pivotBInW) {
- memnew_placement(&jacLinear, JacobianEntry3DSW(
+ memnew_placement(&jacLinear, GodotJacobianEntry3D(
A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(),
pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
@@ -292,16 +291,16 @@ void Generic6DOFJoint3DSW::buildLinearJacobian(
B->get_inv_mass()));
}
-void Generic6DOFJoint3DSW::buildAngularJacobian(
- JacobianEntry3DSW &jacAngular, const Vector3 &jointAxisW) {
- memnew_placement(&jacAngular, JacobianEntry3DSW(jointAxisW,
+void GodotGeneric6DOFJoint3D::buildAngularJacobian(
+ GodotJacobianEntry3D &jacAngular, const Vector3 &jointAxisW) {
+ memnew_placement(&jacAngular, GodotJacobianEntry3D(jointAxisW,
A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(),
A->get_inv_inertia(),
B->get_inv_inertia()));
}
-bool Generic6DOFJoint3DSW::testAngularLimitMotor(int axis_index) {
+bool GodotGeneric6DOFJoint3D::testAngularLimitMotor(int axis_index) {
real_t angle = m_calculatedAxisAngleDiff[axis_index];
//test limits
@@ -309,7 +308,7 @@ bool Generic6DOFJoint3DSW::testAngularLimitMotor(int axis_index) {
return m_angularLimits[axis_index].needApplyTorques();
}
-bool Generic6DOFJoint3DSW::setup(real_t p_timestep) {
+bool GodotGeneric6DOFJoint3D::setup(real_t p_timestep) {
dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
@@ -365,7 +364,7 @@ bool Generic6DOFJoint3DSW::setup(real_t p_timestep) {
return true;
}
-void Generic6DOFJoint3DSW::solve(real_t p_timestep) {
+void GodotGeneric6DOFJoint3D::solve(real_t p_timestep) {
m_timeStep = p_timestep;
//calculateTransforms();
@@ -414,19 +413,19 @@ void Generic6DOFJoint3DSW::solve(real_t p_timestep) {
}
}
-void Generic6DOFJoint3DSW::updateRHS(real_t timeStep) {
+void GodotGeneric6DOFJoint3D::updateRHS(real_t timeStep) {
(void)timeStep;
}
-Vector3 Generic6DOFJoint3DSW::getAxis(int axis_index) const {
+Vector3 GodotGeneric6DOFJoint3D::getAxis(int axis_index) const {
return m_calculatedAxis[axis_index];
}
-real_t Generic6DOFJoint3DSW::getAngle(int axis_index) const {
+real_t GodotGeneric6DOFJoint3D::getAngle(int axis_index) const {
return m_calculatedAxisAngleDiff[axis_index];
}
-void Generic6DOFJoint3DSW::calcAnchorPos() {
+void GodotGeneric6DOFJoint3D::calcAnchorPos() {
real_t imA = A->get_inv_mass();
real_t imB = B->get_inv_mass();
real_t weight;
@@ -438,9 +437,9 @@ void Generic6DOFJoint3DSW::calcAnchorPos() {
const Vector3 &pA = m_calculatedTransformA.origin;
const Vector3 &pB = m_calculatedTransformB.origin;
m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight);
-} // Generic6DOFJointSW::calcAnchorPos()
+}
-void Generic6DOFJoint3DSW::set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) {
+void GodotGeneric6DOFJoint3D::set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param, real_t p_value) {
ERR_FAIL_INDEX(p_axis, 3);
switch (p_param) {
case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
@@ -527,7 +526,7 @@ void Generic6DOFJoint3DSW::set_param(Vector3::Axis p_axis, PhysicsServer3D::G6DO
}
}
-real_t Generic6DOFJoint3DSW::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const {
+real_t GodotGeneric6DOFJoint3D::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisParam p_param) const {
ERR_FAIL_INDEX_V(p_axis, 3, 0);
switch (p_param) {
case PhysicsServer3D::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
@@ -615,7 +614,7 @@ real_t Generic6DOFJoint3DSW::get_param(Vector3::Axis p_axis, PhysicsServer3D::G6
return 0;
}
-void Generic6DOFJoint3DSW::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value) {
+void GodotGeneric6DOFJoint3D::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag, bool p_value) {
ERR_FAIL_INDEX(p_axis, 3);
switch (p_flag) {
@@ -642,7 +641,7 @@ void Generic6DOFJoint3DSW::set_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOF
}
}
-bool Generic6DOFJoint3DSW::get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const {
+bool GodotGeneric6DOFJoint3D::get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const {
ERR_FAIL_INDEX_V(p_axis, 3, 0);
switch (p_flag) {
case PhysicsServer3D::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
diff --git a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h
index 6492e40393..729b3fa1f9 100644
--- a/servers/physics_3d/joints/generic_6dof_joint_3d_sw.h
+++ b/servers/physics_3d/joints/godot_generic_6dof_joint_3d.h
@@ -1,5 +1,5 @@
/*************************************************************************/
-/* generic_6dof_joint_3d_sw.h */
+/* godot_generic_6dof_joint_3d.h */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@@ -32,11 +32,11 @@
Adapted to Godot from the Bullet library.
*/
-#ifndef GENERIC_6DOF_JOINT_SW_H
-#define GENERIC_6DOF_JOINT_SW_H
+#ifndef GODOT_GENERIC_6DOF_JOINT_3D_H
+#define GODOT_GENERIC_6DOF_JOINT_3D_H
-#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h"
-#include "servers/physics_3d/joints_3d_sw.h"
+#include "servers/physics_3d/godot_joint_3d.h"
+#include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
/*
Bullet Continuous Collision Detection and Physics Library
@@ -55,13 +55,13 @@ subject to the following restrictions:
/*
2007-09-09
-Generic6DOFJointSW Refactored by Francisco Le?n
+GodotGeneric6DOFJoint3D Refactored by Francisco Le?n
email: projectileman@yahoo.com
http://gimpact.sf.net
*/
//! Rotation Limit structure for generic joints
-class G6DOFRotationalLimitMotor3DSW {
+class GodotG6DOFRotationalLimitMotor3D {
public:
//! limit_parameters
//!@{
@@ -86,29 +86,25 @@ public:
real_t m_accumulatedImpulse = 0.0;
//!@}
- G6DOFRotationalLimitMotor3DSW() {}
+ GodotG6DOFRotationalLimitMotor3D() {}
- //! Is limited
bool isLimited() {
return (m_loLimit < m_hiLimit);
}
- //! Need apply correction
+ // Need apply correction.
bool needApplyTorques() {
return (m_enableMotor || m_currentLimit != 0);
}
- //! calculates error
- /*!
- calculates m_currentLimit and m_currentLimitError.
- */
+ // Calculates m_currentLimit and m_currentLimitError.
int testLimitValue(real_t test_value);
- //! apply the correction impulses for two bodies
- real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, Body3DSW *body0, Body3DSW *body1, bool p_body0_dynamic, bool p_body1_dynamic);
+ // Apply the correction impulses for two bodies.
+ real_t solveAngularLimits(real_t timeStep, Vector3 &axis, real_t jacDiagABInv, GodotBody3D *body0, GodotBody3D *body1, bool p_body0_dynamic, bool p_body1_dynamic);
};
-class G6DOFTranslationalLimitMotor3DSW {
+class GodotG6DOFTranslationalLimitMotor3D {
public:
Vector3 m_lowerLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint lower limits
Vector3 m_upperLimit = Vector3(0.0, 0.0, 0.0); //!< the constraint upper limits
@@ -135,23 +131,23 @@ public:
real_t solveLinearAxis(
real_t timeStep,
real_t jacDiagABInv,
- Body3DSW *body1, const Vector3 &pointInA,
- Body3DSW *body2, const Vector3 &pointInB,
+ GodotBody3D *body1, const Vector3 &pointInA,
+ GodotBody3D *body2, const Vector3 &pointInB,
bool p_body1_dynamic, bool p_body2_dynamic,
int limit_index,
const Vector3 &axis_normal_on_a,
const Vector3 &anchorPos);
};
-class Generic6DOFJoint3DSW : public Joint3DSW {
+class GodotGeneric6DOFJoint3D : public GodotJoint3D {
protected:
union {
struct {
- Body3DSW *A;
- Body3DSW *B;
+ GodotBody3D *A;
+ GodotBody3D *B;
};
- Body3DSW *_arr[2] = { nullptr, nullptr };
+ GodotBody3D *_arr[2] = { nullptr, nullptr };
};
//! relative_frames
@@ -162,18 +158,18 @@ protected:
//! Jacobians
//!@{
- JacobianEntry3DSW m_jacLinear[3]; //!< 3 orthogonal linear constraints
- JacobianEntry3DSW m_jacAng[3]; //!< 3 orthogonal angular constraints
+ GodotJacobianEntry3D m_jacLinear[3]; //!< 3 orthogonal linear constraints
+ GodotJacobianEntry3D m_jacAng[3]; //!< 3 orthogonal angular constraints
//!@}
//! Linear_Limit_parameters
//!@{
- G6DOFTranslationalLimitMotor3DSW m_linearLimits;
+ GodotG6DOFTranslationalLimitMotor3D m_linearLimits;
//!@}
//! hinge_parameters
//!@{
- G6DOFRotationalLimitMotor3DSW m_angularLimits[3];
+ GodotG6DOFRotationalLimitMotor3D m_angularLimits[3];
//!@}
protected:
@@ -191,45 +187,35 @@ protected:
//!@}
- Generic6DOFJoint3DSW(Generic6DOFJoint3DSW const &) = delete;
- void operator=(Generic6DOFJoint3DSW const &) = delete;
+ GodotGeneric6DOFJoint3D(GodotGeneric6DOFJoint3D const &) = delete;
+ void operator=(GodotGeneric6DOFJoint3D const &) = delete;
void buildLinearJacobian(
- JacobianEntry3DSW &jacLinear, const Vector3 &normalWorld,
+ GodotJacobianEntry3D &jacLinear, const Vector3 &normalWorld,
const Vector3 &pivotAInW, const Vector3 &pivotBInW);
- void buildAngularJacobian(JacobianEntry3DSW &jacAngular, const Vector3 &jointAxisW);
+ void buildAngularJacobian(GodotJacobianEntry3D &jacAngular, const Vector3 &jointAxisW);
//! calcs the euler angles between the two bodies.
void calculateAngleInfo();
public:
- Generic6DOFJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA);
+ GodotGeneric6DOFJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB, bool useLinearReferenceFrameA);
virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_6DOF; }
virtual bool setup(real_t p_step) override;
virtual void solve(real_t p_step) override;
- //! Calcs global transform of the offsets
- /*!
- Calcs the global transform for the joint offset for body A an B, and also calcs the agle differences between the bodies.
- \sa Generic6DOFJointSW.getCalculatedTransformA , Generic6DOFJointSW.getCalculatedTransformB, Generic6DOFJointSW.calculateAngleInfo
- */
+ // Calcs the global transform for the joint offset for body A an B, and also calcs the angle differences between the bodies.
void calculateTransforms();
- //! Gets the global transform of the offset for body A
- /*!
- \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo.
- */
+ // Gets the global transform of the offset for body A.
const Transform3D &getCalculatedTransformA() const {
return m_calculatedTransformA;
}
- //! Gets the global transform of the offset for body B
- /*!
- \sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo.
- */
+ // Gets the global transform of the offset for body B.
const Transform3D &getCalculatedTransformB() const {
return m_calculatedTransformB;
}
@@ -250,27 +236,16 @@ public:
return m_frameInB;
}
- //! performs Jacobian calculation, and also calculates angle differences and axis
-
+ // Performs Jacobian calculation, and also calculates angle differences and axis.
void updateRHS(real_t timeStep);
- //! Get the rotation axis in global coordinates
- /*!
- \pre Generic6DOFJointSW.buildJacobian must be called previously.
- */
+ // Get the rotation axis in global coordinates.
Vector3 getAxis(int axis_index) const;
- //! Get the relative Euler angle
- /*!
- \pre Generic6DOFJointSW.buildJacobian must be called previously.
- */
+ // Get the relative Euler angle.
real_t getAngle(int axis_index) const;
- //! Test angular limit.
- /*!
- Calculates angular correction and returns true if limit needs to be corrected.
- \pre Generic6DOFJointSW.buildJacobian must be called previously.
- */
+ // Calculates angular correction and returns true if limit needs to be corrected.
bool testAngularLimitMotor(int axis_index);
void setLinearLowerLimit(const Vector3 &linearLower) {
@@ -293,17 +268,17 @@ public:
m_angularLimits[2].m_hiLimit = angularUpper.z;
}
- //! Retrieves the angular limit information.
- G6DOFRotationalLimitMotor3DSW *getRotationalLimitMotor(int index) {
+ // Retrieves the angular limit information.
+ GodotG6DOFRotationalLimitMotor3D *getRotationalLimitMotor(int index) {
return &m_angularLimits[index];
}
- //! Retrieves the limit information.
- G6DOFTranslationalLimitMotor3DSW *getTranslationalLimitMotor() {
+ // Retrieves the limit information.
+ GodotG6DOFTranslationalLimitMotor3D *getTranslationalLimitMotor() {
return &m_linearLimits;
}
- //first 3 are linear, next 3 are angular
+ // First 3 are linear, next 3 are angular.
void setLimit(int axis, real_t lo, real_t hi) {
if (axis < 3) {
m_linearLimits.m_lowerLimit[axis] = lo;
@@ -328,10 +303,10 @@ public:
return m_angularLimits[limitIndex - 3].isLimited();
}
- const Body3DSW *getRigidBodyA() const {
+ const GodotBody3D *getRigidBodyA() const {
return A;
}
- const Body3DSW *getRigidBodyB() const {
+ const GodotBody3D *getRigidBodyB() const {
return B;
}
@@ -344,4 +319,4 @@ public:
bool get_flag(Vector3::Axis p_axis, PhysicsServer3D::G6DOFJointAxisFlag p_flag) const;
};
-#endif // GENERIC_6DOF_JOINT_SW_H
+#endif // GODOT_GENERIC_6DOF_JOINT_3D_H
diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp b/servers/physics_3d/joints/godot_hinge_joint_3d.cpp
index a45fcf7eb5..7b7ca1b3ac 100644
--- a/servers/physics_3d/joints/hinge_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/godot_hinge_joint_3d.cpp
@@ -1,5 +1,5 @@
/*************************************************************************/
-/* hinge_joint_3d_sw.cpp */
+/* godot_hinge_joint_3d.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@@ -47,7 +47,7 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-#include "hinge_joint_3d_sw.h"
+#include "godot_hinge_joint_3d.h"
static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) {
if (Math::abs(n.z) > Math_SQRT12) {
@@ -67,8 +67,8 @@ static void plane_space(const Vector3 &n, Vector3 &p, Vector3 &q) {
}
}
-HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameA, const Transform3D &frameB) :
- Joint3DSW(_arr, 2) {
+GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameA, const Transform3D &frameB) :
+ GodotJoint3D(_arr, 2) {
A = rbA;
B = rbB;
@@ -83,9 +83,9 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &
B->add_constraint(this, 1);
}
-HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB,
+GodotHingeJoint3D::GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB,
const Vector3 &axisInA, const Vector3 &axisInB) :
- Joint3DSW(_arr, 2) {
+ GodotJoint3D(_arr, 2) {
A = rbA;
B = rbB;
@@ -124,7 +124,7 @@ HingeJoint3DSW::HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivo
B->add_constraint(this, 1);
}
-bool HingeJoint3DSW::setup(real_t p_step) {
+bool GodotHingeJoint3D::setup(real_t p_step) {
dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
@@ -149,7 +149,7 @@ bool HingeJoint3DSW::setup(real_t p_step) {
plane_space(normal[0], normal[1], normal[2]);
for (int i = 0; i < 3; i++) {
- memnew_placement(&m_jac[i], JacobianEntry3DSW(
+ memnew_placement(&m_jac[i], GodotJacobianEntry3D(
A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(),
pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
@@ -175,19 +175,19 @@ bool HingeJoint3DSW::setup(real_t p_step) {
Vector3 jointAxis1 = A->get_transform().basis.xform(jointAxis1local);
Vector3 hingeAxisWorld = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(2));
- memnew_placement(&m_jacAng[0], JacobianEntry3DSW(jointAxis0,
+ memnew_placement(&m_jacAng[0], GodotJacobianEntry3D(jointAxis0,
A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(),
A->get_inv_inertia(),
B->get_inv_inertia()));
- memnew_placement(&m_jacAng[1], JacobianEntry3DSW(jointAxis1,
+ memnew_placement(&m_jacAng[1], GodotJacobianEntry3D(jointAxis1,
A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(),
A->get_inv_inertia(),
B->get_inv_inertia()));
- memnew_placement(&m_jacAng[2], JacobianEntry3DSW(hingeAxisWorld,
+ memnew_placement(&m_jacAng[2], GodotJacobianEntry3D(hingeAxisWorld,
A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(),
A->get_inv_inertia(),
@@ -226,7 +226,7 @@ bool HingeJoint3DSW::setup(real_t p_step) {
return true;
}
-void HingeJoint3DSW::solve(real_t p_step) {
+void GodotHingeJoint3D::solve(real_t p_step) {
Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
@@ -377,7 +377,7 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) {
return (y < 0.0f) ? -angle : angle;
}
-real_t HingeJoint3DSW::get_hinge_angle() {
+real_t GodotHingeJoint3D::get_hinge_angle() {
const Vector3 refAxis0 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(0));
const Vector3 refAxis1 = A->get_transform().basis.xform(m_rbAFrame.basis.get_axis(1));
const Vector3 swingAxis = B->get_transform().basis.xform(m_rbBFrame.basis.get_axis(1));
@@ -385,7 +385,7 @@ real_t HingeJoint3DSW::get_hinge_angle() {
return atan2fast(swingAxis.dot(refAxis0), swingAxis.dot(refAxis1));
}
-void HingeJoint3DSW::set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value) {
+void GodotHingeJoint3D::set_param(PhysicsServer3D::HingeJointParam p_param, real_t p_value) {
switch (p_param) {
case PhysicsServer3D::HINGE_JOINT_BIAS:
tau = p_value;
@@ -416,7 +416,7 @@ void HingeJoint3DSW::set_param(PhysicsServer3D::HingeJointParam p_param, real_t
}
}
-real_t HingeJoint3DSW::get_param(PhysicsServer3D::HingeJointParam p_param) const {
+real_t GodotHingeJoint3D::get_param(PhysicsServer3D::HingeJointParam p_param) const {
switch (p_param) {
case PhysicsServer3D::HINGE_JOINT_BIAS:
return tau;
@@ -441,7 +441,7 @@ real_t HingeJoint3DSW::get_param(PhysicsServer3D::HingeJointParam p_param) const
return 0;
}
-void HingeJoint3DSW::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value) {
+void GodotHingeJoint3D::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value) {
switch (p_flag) {
case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT:
m_useLimit = p_value;
@@ -454,7 +454,7 @@ void HingeJoint3DSW::set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_val
}
}
-bool HingeJoint3DSW::get_flag(PhysicsServer3D::HingeJointFlag p_flag) const {
+bool GodotHingeJoint3D::get_flag(PhysicsServer3D::HingeJointFlag p_flag) const {
switch (p_flag) {
case PhysicsServer3D::HINGE_JOINT_FLAG_USE_LIMIT:
return m_useLimit;
diff --git a/servers/physics_3d/joints/hinge_joint_3d_sw.h b/servers/physics_3d/joints/godot_hinge_joint_3d.h
index a4ceff9ffe..ff1fbe0f25 100644
--- a/servers/physics_3d/joints/hinge_joint_3d_sw.h
+++ b/servers/physics_3d/joints/godot_hinge_joint_3d.h
@@ -1,5 +1,5 @@
/*************************************************************************/
-/* hinge_joint_3d_sw.h */
+/* godot_hinge_joint_3d.h */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@@ -32,11 +32,11 @@
Adapted to Godot from the Bullet library.
*/
-#ifndef HINGE_JOINT_SW_H
-#define HINGE_JOINT_SW_H
+#ifndef GODOT_HINGE_JOINT_3D_H
+#define GODOT_HINGE_JOINT_3D_H
-#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h"
-#include "servers/physics_3d/joints_3d_sw.h"
+#include "servers/physics_3d/godot_joint_3d.h"
+#include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
/*
Bullet Continuous Collision Detection and Physics Library
@@ -53,18 +53,18 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-class HingeJoint3DSW : public Joint3DSW {
+class GodotHingeJoint3D : public GodotJoint3D {
union {
struct {
- Body3DSW *A;
- Body3DSW *B;
+ GodotBody3D *A;
+ GodotBody3D *B;
};
- Body3DSW *_arr[2] = {};
+ GodotBody3D *_arr[2] = {};
};
- JacobianEntry3DSW m_jac[3]; //3 orthogonal linear constraints
- JacobianEntry3DSW m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
+ GodotJacobianEntry3D m_jac[3]; //3 orthogonal linear constraints
+ GodotJacobianEntry3D m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
Transform3D m_rbAFrame; // constraint axii. Assumes z is hinge axis.
Transform3D m_rbBFrame;
@@ -109,8 +109,8 @@ public:
void set_flag(PhysicsServer3D::HingeJointFlag p_flag, bool p_value);
bool get_flag(PhysicsServer3D::HingeJointFlag p_flag) const;
- HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameA, const Transform3D &frameB);
- HingeJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB);
+ GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameA, const Transform3D &frameB);
+ GodotHingeJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Vector3 &pivotInA, const Vector3 &pivotInB, const Vector3 &axisInA, const Vector3 &axisInB);
};
-#endif // HINGE_JOINT_SW_H
+#endif // GODOT_HINGE_JOINT_3D_H
diff --git a/servers/physics_3d/joints/jacobian_entry_3d_sw.h b/servers/physics_3d/joints/godot_jacobian_entry_3d.h
index 7294ff78e3..90a77a9b61 100644
--- a/servers/physics_3d/joints/jacobian_entry_3d_sw.h
+++ b/servers/physics_3d/joints/godot_jacobian_entry_3d.h
@@ -1,5 +1,5 @@
/*************************************************************************/
-/* jacobian_entry_3d_sw.h */
+/* godot_jacobian_entry_3d.h */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@@ -32,8 +32,8 @@
Adapted to Godot from the Bullet library.
*/
-#ifndef JACOBIAN_ENTRY_SW_H
-#define JACOBIAN_ENTRY_SW_H
+#ifndef GODOT_JACOBIAN_ENTRY_3D_H
+#define GODOT_JACOBIAN_ENTRY_3D_H
/*
Bullet Continuous Collision Detection and Physics Library
@@ -52,11 +52,11 @@ subject to the following restrictions:
#include "core/math/transform_3d.h"
-class JacobianEntry3DSW {
+class GodotJacobianEntry3D {
public:
- JacobianEntry3DSW() {}
+ GodotJacobianEntry3D() {}
//constraint between two different rigidbodies
- JacobianEntry3DSW(
+ GodotJacobianEntry3D(
const Basis &world2A,
const Basis &world2B,
const Vector3 &rel_pos1, const Vector3 &rel_pos2,
@@ -76,7 +76,7 @@ public:
}
//angular constraint between two different rigidbodies
- JacobianEntry3DSW(const Vector3 &jointAxis,
+ GodotJacobianEntry3D(const Vector3 &jointAxis,
const Basis &world2A,
const Basis &world2B,
const Vector3 &inertiaInvA,
@@ -92,7 +92,7 @@ public:
}
//angular constraint between two different rigidbodies
- JacobianEntry3DSW(const Vector3 &axisInA,
+ GodotJacobianEntry3D(const Vector3 &axisInA,
const Vector3 &axisInB,
const Vector3 &inertiaInvA,
const Vector3 &inertiaInvB) :
@@ -107,7 +107,7 @@ public:
}
//constraint on one rigidbody
- JacobianEntry3DSW(
+ GodotJacobianEntry3D(
const Basis &world2A,
const Vector3 &rel_pos1, const Vector3 &rel_pos2,
const Vector3 &jointAxis,
@@ -126,16 +126,16 @@ public:
real_t getDiagonal() const { return m_Adiag; }
// for two constraints on the same rigidbody (for example vehicle friction)
- real_t getNonDiagonal(const JacobianEntry3DSW &jacB, const real_t massInvA) const {
- const JacobianEntry3DSW &jacA = *this;
+ real_t getNonDiagonal(const GodotJacobianEntry3D &jacB, const real_t massInvA) const {
+ const GodotJacobianEntry3D &jacA = *this;
real_t lin = massInvA * jacA.m_linearJointAxis.dot(jacB.m_linearJointAxis);
real_t ang = jacA.m_0MinvJt.dot(jacB.m_aJ);
return lin + ang;
}
// for two constraints on sharing two same rigidbodies (for example two contact points between two rigidbodies)
- real_t getNonDiagonal(const JacobianEntry3DSW &jacB, const real_t massInvA, const real_t massInvB) const {
- const JacobianEntry3DSW &jacA = *this;
+ real_t getNonDiagonal(const GodotJacobianEntry3D &jacB, const real_t massInvA, const real_t massInvB) const {
+ const GodotJacobianEntry3D &jacA = *this;
Vector3 lin = jacA.m_linearJointAxis * jacB.m_linearJointAxis;
Vector3 ang0 = jacA.m_0MinvJt * jacB.m_aJ;
Vector3 ang1 = jacA.m_1MinvJt * jacB.m_bJ;
@@ -166,4 +166,4 @@ public:
real_t m_Adiag = 1.0;
};
-#endif // JACOBIAN_ENTRY_SW_H
+#endif // GODOT_JACOBIAN_ENTRY_3D_H
diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.cpp b/servers/physics_3d/joints/godot_pin_joint_3d.cpp
index f41151ec0e..10d52ad5e9 100644
--- a/servers/physics_3d/joints/pin_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/godot_pin_joint_3d.cpp
@@ -1,5 +1,5 @@
/*************************************************************************/
-/* pin_joint_3d_sw.cpp */
+/* godot_pin_joint_3d.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@@ -47,9 +47,9 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-#include "pin_joint_3d_sw.h"
+#include "godot_pin_joint_3d.h"
-bool PinJoint3DSW::setup(real_t p_step) {
+bool GodotPinJoint3D::setup(real_t p_step) {
dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
@@ -63,7 +63,7 @@ bool PinJoint3DSW::setup(real_t p_step) {
for (int i = 0; i < 3; i++) {
normal[i] = 1;
- memnew_placement(&m_jac[i], JacobianEntry3DSW(
+ memnew_placement(&m_jac[i], GodotJacobianEntry3D(
A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(),
A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(),
@@ -79,7 +79,7 @@ bool PinJoint3DSW::setup(real_t p_step) {
return true;
}
-void PinJoint3DSW::solve(real_t p_step) {
+void GodotPinJoint3D::solve(real_t p_step) {
Vector3 pivotAInW = A->get_transform().xform(m_pivotInA);
Vector3 pivotBInW = B->get_transform().xform(m_pivotInB);
@@ -137,7 +137,7 @@ void PinJoint3DSW::solve(real_t p_step) {
}
}
-void PinJoint3DSW::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value) {
+void GodotPinJoint3D::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_value) {
switch (p_param) {
case PhysicsServer3D::PIN_JOINT_BIAS:
m_tau = p_value;
@@ -151,7 +151,7 @@ void PinJoint3DSW::set_param(PhysicsServer3D::PinJointParam p_param, real_t p_va
}
}
-real_t PinJoint3DSW::get_param(PhysicsServer3D::PinJointParam p_param) const {
+real_t GodotPinJoint3D::get_param(PhysicsServer3D::PinJointParam p_param) const {
switch (p_param) {
case PhysicsServer3D::PIN_JOINT_BIAS:
return m_tau;
@@ -164,8 +164,8 @@ real_t PinJoint3DSW::get_param(PhysicsServer3D::PinJointParam p_param) const {
return 0;
}
-PinJoint3DSW::PinJoint3DSW(Body3DSW *p_body_a, const Vector3 &p_pos_a, Body3DSW *p_body_b, const Vector3 &p_pos_b) :
- Joint3DSW(_arr, 2) {
+GodotPinJoint3D::GodotPinJoint3D(GodotBody3D *p_body_a, const Vector3 &p_pos_a, GodotBody3D *p_body_b, const Vector3 &p_pos_b) :
+ GodotJoint3D(_arr, 2) {
A = p_body_a;
B = p_body_b;
m_pivotInA = p_pos_a;
@@ -175,5 +175,5 @@ PinJoint3DSW::PinJoint3DSW(Body3DSW *p_body_a, const Vector3 &p_pos_a, Body3DSW
B->add_constraint(this, 1);
}
-PinJoint3DSW::~PinJoint3DSW() {
+GodotPinJoint3D::~GodotPinJoint3D() {
}
diff --git a/servers/physics_3d/joints/pin_joint_3d_sw.h b/servers/physics_3d/joints/godot_pin_joint_3d.h
index 79af48f2a5..17e2e6e973 100644
--- a/servers/physics_3d/joints/pin_joint_3d_sw.h
+++ b/servers/physics_3d/joints/godot_pin_joint_3d.h
@@ -1,5 +1,5 @@
/*************************************************************************/
-/* pin_joint_3d_sw.h */
+/* godot_pin_joint_3d.h */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@@ -32,11 +32,11 @@
Adapted to Godot from the Bullet library.
*/
-#ifndef PIN_JOINT_SW_H
-#define PIN_JOINT_SW_H
+#ifndef GODOT_PIN_JOINT_3D_H
+#define GODOT_PIN_JOINT_3D_H
-#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h"
-#include "servers/physics_3d/joints_3d_sw.h"
+#include "servers/physics_3d/godot_joint_3d.h"
+#include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
/*
Bullet Continuous Collision Detection and Physics Library
@@ -53,14 +53,14 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
-class PinJoint3DSW : public Joint3DSW {
+class GodotPinJoint3D : public GodotJoint3D {
union {
struct {
- Body3DSW *A;
- Body3DSW *B;
+ GodotBody3D *A;
+ GodotBody3D *B;
};
- Body3DSW *_arr[2] = {};
+ GodotBody3D *_arr[2] = {};
};
real_t m_tau = 0.3; //bias
@@ -68,7 +68,7 @@ class PinJoint3DSW : public Joint3DSW {
real_t m_impulseClamp = 0.0;
real_t m_appliedImpulse = 0.0;
- JacobianEntry3DSW m_jac[3] = {}; //3 orthogonal linear constraints
+ GodotJacobianEntry3D m_jac[3] = {}; //3 orthogonal linear constraints
Vector3 m_pivotInA;
Vector3 m_pivotInB;
@@ -88,8 +88,8 @@ public:
Vector3 get_position_a() { return m_pivotInA; }
Vector3 get_position_b() { return m_pivotInB; }
- PinJoint3DSW(Body3DSW *p_body_a, const Vector3 &p_pos_a, Body3DSW *p_body_b, const Vector3 &p_pos_b);
- ~PinJoint3DSW();
+ GodotPinJoint3D(GodotBody3D *p_body_a, const Vector3 &p_pos_a, GodotBody3D *p_body_b, const Vector3 &p_pos_b);
+ ~GodotPinJoint3D();
};
-#endif // PIN_JOINT_SW_H
+#endif // GODOT_PIN_JOINT_3D_H
diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.cpp b/servers/physics_3d/joints/godot_slider_joint_3d.cpp
index e10ed436d5..3be111ac92 100644
--- a/servers/physics_3d/joints/slider_joint_3d_sw.cpp
+++ b/servers/physics_3d/joints/godot_slider_joint_3d.cpp
@@ -1,5 +1,5 @@
/*************************************************************************/
-/* slider_joint_3d_sw.cpp */
+/* godot_slider_joint_3d.cpp */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@@ -53,7 +53,7 @@ April 04, 2008
*/
-#include "slider_joint_3d_sw.h"
+#include "godot_slider_joint_3d.h"
//-----------------------------------------------------------------------------
@@ -76,8 +76,8 @@ static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x) {
//-----------------------------------------------------------------------------
-SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB) :
- Joint3DSW(_arr, 2),
+GodotSliderJoint3D::GodotSliderJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB) :
+ GodotJoint3D(_arr, 2),
m_frameInA(frameInA),
m_frameInB(frameInB) {
A = rbA;
@@ -85,11 +85,11 @@ SliderJoint3DSW::SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D
A->add_constraint(this, 0);
B->add_constraint(this, 1);
-} // SliderJointSW::SliderJointSW()
+}
//-----------------------------------------------------------------------------
-bool SliderJoint3DSW::setup(real_t p_step) {
+bool GodotSliderJoint3D::setup(real_t p_step) {
dynamic_A = (A->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
dynamic_B = (B->get_mode() > PhysicsServer3D::BODY_MODE_KINEMATIC);
@@ -112,7 +112,7 @@ bool SliderJoint3DSW::setup(real_t p_step) {
//linear part
for (i = 0; i < 3; i++) {
normalWorld = m_calculatedTransformA.basis.get_axis(i);
- memnew_placement(&m_jacLin[i], JacobianEntry3DSW(
+ memnew_placement(&m_jacLin[i], GodotJacobianEntry3D(
A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(),
m_relPosA - A->get_center_of_mass(),
@@ -129,7 +129,7 @@ bool SliderJoint3DSW::setup(real_t p_step) {
// angular part
for (i = 0; i < 3; i++) {
normalWorld = m_calculatedTransformA.basis.get_axis(i);
- memnew_placement(&m_jacAng[i], JacobianEntry3DSW(
+ memnew_placement(&m_jacAng[i], GodotJacobianEntry3D(
normalWorld,
A->get_principal_inertia_axes().transposed(),
B->get_principal_inertia_axes().transposed(),
@@ -144,11 +144,11 @@ bool SliderJoint3DSW::setup(real_t p_step) {
m_accumulatedAngMotorImpulse = real_t(0.0);
return true;
-} // SliderJointSW::buildJacobianInt()
+}
//-----------------------------------------------------------------------------
-void SliderJoint3DSW::solve(real_t p_step) {
+void GodotSliderJoint3D::solve(real_t p_step) {
int i;
// linear
Vector3 velA = A->get_velocity_in_local_point(m_relPosA);
@@ -284,13 +284,11 @@ void SliderJoint3DSW::solve(real_t p_step) {
}
}
}
-} // SliderJointSW::solveConstraint()
-
-//-----------------------------------------------------------------------------
+}
//-----------------------------------------------------------------------------
-void SliderJoint3DSW::calculateTransforms() {
+void GodotSliderJoint3D::calculateTransforms() {
m_calculatedTransformA = A->get_transform() * m_frameInA;
m_calculatedTransformB = B->get_transform() * m_frameInB;
m_realPivotAInW = m_calculatedTransformA.origin;
@@ -305,11 +303,11 @@ void SliderJoint3DSW::calculateTransforms() {
normalWorld = m_calculatedTransformA.basis.get_axis(i);
m_depth[i] = m_delta.dot(normalWorld);
}
-} // SliderJointSW::calculateTransforms()
+}
//-----------------------------------------------------------------------------
-void SliderJoint3DSW::testLinLimits() {
+void GodotSliderJoint3D::testLinLimits() {
m_solveLinLim = false;
m_linPos = m_depth[0];
if (m_lowerLinLimit <= m_upperLinLimit) {
@@ -325,11 +323,11 @@ void SliderJoint3DSW::testLinLimits() {
} else {
m_depth[0] = real_t(0.);
}
-} // SliderJointSW::testLinLimits()
+}
//-----------------------------------------------------------------------------
-void SliderJoint3DSW::testAngLimits() {
+void GodotSliderJoint3D::testAngLimits() {
m_angDepth = real_t(0.);
m_solveAngLim = false;
if (m_lowerAngLimit <= m_upperAngLimit) {
@@ -345,26 +343,26 @@ void SliderJoint3DSW::testAngLimits() {
m_solveAngLim = true;
}
}
-} // SliderJointSW::testAngLimits()
+}
//-----------------------------------------------------------------------------
-Vector3 SliderJoint3DSW::getAncorInA() {
+Vector3 GodotSliderJoint3D::getAncorInA() {
Vector3 ancorInA;
ancorInA = m_realPivotAInW + (m_lowerLinLimit + m_upperLinLimit) * real_t(0.5) * m_sliderAxis;
ancorInA = A->get_transform().inverse().xform(ancorInA);
return ancorInA;
-} // SliderJointSW::getAncorInA()
+}
//-----------------------------------------------------------------------------
-Vector3 SliderJoint3DSW::getAncorInB() {
+Vector3 GodotSliderJoint3D::getAncorInB() {
Vector3 ancorInB;
ancorInB = m_frameInB.origin;
return ancorInB;
-} // SliderJointSW::getAncorInB();
+}
-void SliderJoint3DSW::set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value) {
+void GodotSliderJoint3D::set_param(PhysicsServer3D::SliderJointParam p_param, real_t p_value) {
switch (p_param) {
case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER:
m_upperLinLimit = p_value;
@@ -439,7 +437,7 @@ void SliderJoint3DSW::set_param(PhysicsServer3D::SliderJointParam p_param, real_
}
}
-real_t SliderJoint3DSW::get_param(PhysicsServer3D::SliderJointParam p_param) const {
+real_t GodotSliderJoint3D::get_param(PhysicsServer3D::SliderJointParam p_param) const {
switch (p_param) {
case PhysicsServer3D::SLIDER_JOINT_LINEAR_LIMIT_UPPER:
return m_upperLinLimit;
diff --git a/servers/physics_3d/joints/slider_joint_3d_sw.h b/servers/physics_3d/joints/godot_slider_joint_3d.h
index d32ad9469e..9baaf1fa40 100644
--- a/servers/physics_3d/joints/slider_joint_3d_sw.h
+++ b/servers/physics_3d/joints/godot_slider_joint_3d.h
@@ -1,5 +1,5 @@
/*************************************************************************/
-/* slider_joint_3d_sw.h */
+/* godot_slider_joint_3d.h */
/*************************************************************************/
/* This file is part of: */
/* GODOT ENGINE */
@@ -32,11 +32,11 @@
Adapted to Godot from the Bullet library.
*/
-#ifndef SLIDER_JOINT_SW_H
-#define SLIDER_JOINT_SW_H
+#ifndef GODOT_SLIDER_JOINT_3D_H
+#define GODOT_SLIDER_JOINT_3D_H
-#include "servers/physics_3d/joints/jacobian_entry_3d_sw.h"
-#include "servers/physics_3d/joints_3d_sw.h"
+#include "servers/physics_3d/godot_joint_3d.h"
+#include "servers/physics_3d/joints/godot_jacobian_entry_3d.h"
/*
Bullet Continuous Collision Detection and Physics Library
@@ -65,15 +65,15 @@ April 04, 2008
//-----------------------------------------------------------------------------
-class SliderJoint3DSW : public Joint3DSW {
+class GodotSliderJoint3D : public GodotJoint3D {
protected:
union {
struct {
- Body3DSW *A;
- Body3DSW *B;
+ GodotBody3D *A;
+ GodotBody3D *B;
};
- Body3DSW *_arr[2] = { nullptr, nullptr };
+ GodotBody3D *_arr[2] = { nullptr, nullptr };
};
Transform3D m_frameInA;
@@ -114,10 +114,10 @@ protected:
bool m_solveLinLim = false;
bool m_solveAngLim = false;
- JacobianEntry3DSW m_jacLin[3] = {};
+ GodotJacobianEntry3D m_jacLin[3] = {};
real_t m_jacLinDiagABInv[3] = {};
- JacobianEntry3DSW m_jacAng[3] = {};
+ GodotJacobianEntry3D m_jacAng[3] = {};
real_t m_timeStep = 0.0;
Transform3D m_calculatedTransformA;
@@ -149,13 +149,13 @@ protected:
public:
// constructors
- SliderJoint3DSW(Body3DSW *rbA, Body3DSW *rbB, const Transform3D &frameInA, const Transform3D &frameInB);
+ GodotSliderJoint3D(GodotBody3D *rbA, GodotBody3D *rbB, const Transform3D &frameInA, const Transform3D &frameInB);
//SliderJointSW();
// overrides
// access
- const Body3DSW *getRigidBodyA() const { return A; }
- const Body3DSW *getRigidBodyB() const { return B; }
+ const GodotBody3D *getRigidBodyA() const { return A; }
+ const GodotBody3D *getRigidBodyB() const { return B; }
const Transform3D &getCalculatedTransformA() const { return m_calculatedTransformA; }
const Transform3D &getCalculatedTransformB() const { return m_calculatedTransformB; }
const Transform3D &getFrameOffsetA() const { return m_frameInA; }
@@ -243,4 +243,4 @@ public:
virtual PhysicsServer3D::JointType get_type() const override { return PhysicsServer3D::JOINT_TYPE_SLIDER; }
};
-#endif // SLIDER_JOINT_SW_H
+#endif // GODOT_SLIDER_JOINT_3D_H