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.
This commit is contained in:
PouleyKetchoupp
parent
5bb3dbbedd
commit
cc39dca9f7
86 changed files with 5351 additions and 5390 deletions
|
|
@ -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;
|
||||
|
|
@ -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
|
||||
|
|
@ -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: {
|
||||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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() {
|
||||
}
|
||||
|
|
@ -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
|
||||
|
|
@ -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;
|
||||
|
|
@ -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
|
||||
Loading…
Add table
Add a link
Reference in a new issue