3D Physics Rework, Other Stuff
-=-=-=-=-=-=-=-=-=-=-=-=-=-
3D Physics:
-Fixed "Bounce" parameter in 3D
-Fixed bug affecting Area (sometims it would not detect properly)
-Vehicle Body has seen heavy work
-Added Query API for doing space queries in 3D. Needs some docs though.
-Added JOINTS! Adapted Bullet Joints: and created easy gizmos for setting them up:
-PinJoint
-HingeJoint (with motor)
-SliderJoint
-ConeTwistJoint
-Generic6DOFJoint
-Added OBJECT PICKING! based on the new query API. Any physics object now (Area or Body) has the following signals and virtual functions:
-input_event (mouse or multitouch input over the body)
-mouse_enter (mouse entered the body area)
-mouse_exit (mouse exited body area)
For Area it needs to be activated manually, as it isn't by default (ray goes thru).
Other:
-Begun working on Windows 8 (RT) port. Compiles but does not work yet.
-Added TheoraPlayer library for improved to-texture and portable video support.
-Fixed a few bugs in the renderer, collada importer, collada exporter, etc.
This commit is contained in:
Juan Linietsky
@ -4,4 +4,6 @@ env.add_source_files(env.servers_sources,"*.cpp")
|
||||
|
||||
Export('env')
|
||||
|
||||
SConscript("joints/SCsub")
|
||||
|
||||
|
||||
|
||||
@ -43,6 +43,7 @@ void AreaSW::set_transform(const Transform& p_transform) {
|
||||
get_space()->area_add_to_moved_list(&moved_list);
|
||||
|
||||
_set_transform(p_transform);
|
||||
_set_inv_transform(p_transform.affine_inverse());
|
||||
}
|
||||
|
||||
void AreaSW::set_space(SpaceSW *p_space) {
|
||||
@ -181,6 +182,7 @@ AreaSW::AreaSW() : CollisionObjectSW(TYPE_AREA), monitor_query_list(this), move
|
||||
point_attenuation=1;
|
||||
density=0.1;
|
||||
priority=0;
|
||||
ray_pickable=false;
|
||||
|
||||
|
||||
}
|
||||
|
||||
@ -48,6 +48,7 @@ class AreaSW : public CollisionObjectSW{
|
||||
float point_attenuation;
|
||||
float density;
|
||||
int priority;
|
||||
bool ray_pickable;
|
||||
|
||||
ObjectID monitor_callback_id;
|
||||
StringName monitor_callback_method;
|
||||
@ -138,6 +139,11 @@ public:
|
||||
_FORCE_INLINE_ void remove_constraint( ConstraintSW* p_constraint) { constraints.erase(p_constraint); }
|
||||
_FORCE_INLINE_ const Set<ConstraintSW*>& get_constraints() const { return constraints; }
|
||||
|
||||
_FORCE_INLINE_ void set_ray_pickable(bool p_enable) { ray_pickable=p_enable; }
|
||||
_FORCE_INLINE_ bool is_ray_pickable() const { return ray_pickable; }
|
||||
|
||||
|
||||
|
||||
void set_transform(const Transform& p_transform);
|
||||
|
||||
void set_space(SpaceSW *p_space);
|
||||
|
||||
@ -298,6 +298,17 @@ bool BodyPairSW::setup(float p_step) {
|
||||
A->apply_bias_impulse( c.rA, -jb_vec );
|
||||
B->apply_bias_impulse( c.rB, jb_vec );
|
||||
|
||||
c.bounce = MAX(A->get_bounce(),B->get_bounce());
|
||||
if (c.bounce) {
|
||||
|
||||
Vector3 crA = A->get_angular_velocity().cross( c.rA );
|
||||
Vector3 crB = B->get_angular_velocity().cross( c.rB );
|
||||
Vector3 dv = B->get_linear_velocity() + crB - A->get_linear_velocity() - crA;
|
||||
//normal impule
|
||||
c.bounce = c.bounce * dv.dot(c.normal);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
return true;
|
||||
@ -350,8 +361,7 @@ void BodyPairSW::solve(float p_step) {
|
||||
|
||||
if (Math::abs(vn)>MIN_VELOCITY) {
|
||||
|
||||
real_t bounce=0;
|
||||
real_t jn = (-bounce -vn)*c.mass_normal;
|
||||
real_t jn = -(c.bounce + vn)*c.mass_normal;
|
||||
real_t jnOld = c.acc_normal_impulse;
|
||||
c.acc_normal_impulse = MAX(jnOld + jn, 0.0f);
|
||||
|
||||
|
||||
@ -61,6 +61,7 @@ class BodyPairSW : public ConstraintSW {
|
||||
real_t acc_bias_impulse; // accumulated normal impulse for position bias (Pnb)
|
||||
real_t mass_normal;
|
||||
real_t bias;
|
||||
real_t bounce;
|
||||
|
||||
real_t depth;
|
||||
bool active;
|
||||
|
||||
@ -236,6 +236,28 @@ public:
|
||||
void integrate_forces(real_t p_step);
|
||||
void integrate_velocities(real_t p_step);
|
||||
|
||||
_FORCE_INLINE_ Vector3 get_velocity_in_local_point(const Vector3& rel_pos) const {
|
||||
|
||||
return linear_velocity + angular_velocity.cross(rel_pos);
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ real_t compute_impulse_denominator(const Vector3& p_pos, const Vector3& p_normal) const {
|
||||
|
||||
Vector3 r0 = p_pos - get_transform().origin;
|
||||
|
||||
Vector3 c0 = (r0).cross(p_normal);
|
||||
|
||||
Vector3 vec = (_inv_inertia_tensor.xform_inv(c0)).cross(r0);
|
||||
|
||||
return _inv_mass + p_normal.dot(vec);
|
||||
|
||||
}
|
||||
|
||||
_FORCE_INLINE_ real_t compute_angular_impulse_denominator(const Vector3& p_axis) const {
|
||||
|
||||
return p_axis.dot( _inv_inertia_tensor.xform_inv(p_axis) );
|
||||
}
|
||||
|
||||
//void simulate_motion(const Transform& p_xform,real_t p_step);
|
||||
void call_queries();
|
||||
void wakeup_neighbours();
|
||||
|
||||
8
servers/physics/joints/SCsub
Normal file
8
servers/physics/joints/SCsub
Normal file
@ -0,0 +1,8 @@
|
||||
Import('env')
|
||||
|
||||
env.add_source_files(env.servers_sources,"*.cpp")
|
||||
|
||||
Export('env')
|
||||
|
||||
|
||||
|
||||
340
servers/physics/joints/cone_twist_joint_sw.cpp
Normal file
340
servers/physics/joints/cone_twist_joint_sw.cpp
Normal file
@ -0,0 +1,340 @@
|
||||
#include "cone_twist_joint_sw.h"
|
||||
|
||||
static void plane_space(const Vector3& n, Vector3& p, Vector3& q) {
|
||||
|
||||
if (Math::abs(n.z) > 0.707106781186547524400844362) {
|
||||
// choose p in y-z plane
|
||||
real_t a = n[1]*n[1] + n[2]*n[2];
|
||||
real_t k = 1.0/Math::sqrt(a);
|
||||
p=Vector3(0,-n[2]*k,n[1]*k);
|
||||
// set q = n x p
|
||||
q=Vector3(a*k,-n[0]*p[2],n[0]*p[1]);
|
||||
}
|
||||
else {
|
||||
// choose p in x-y plane
|
||||
real_t a = n.x*n.x + n.y*n.y;
|
||||
real_t k = 1.0/Math::sqrt(a);
|
||||
p=Vector3(-n.y*k,n.x*k,0);
|
||||
// set q = n x p
|
||||
q=Vector3(-n.z*p.y,n.z*p.x,a*k);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x)
|
||||
{
|
||||
real_t coeff_1 = Math_PI / 4.0f;
|
||||
real_t coeff_2 = 3.0f * coeff_1;
|
||||
real_t abs_y = Math::abs(y);
|
||||
real_t angle;
|
||||
if (x >= 0.0f) {
|
||||
real_t r = (x - abs_y) / (x + abs_y);
|
||||
angle = coeff_1 - coeff_1 * r;
|
||||
} else {
|
||||
real_t r = (x + abs_y) / (abs_y - x);
|
||||
angle = coeff_2 - coeff_1 * r;
|
||||
}
|
||||
return (y < 0.0f) ? -angle : angle;
|
||||
}
|
||||
|
||||
ConeTwistJointSW::ConeTwistJointSW(BodySW* rbA,BodySW* rbB,const Transform& rbAFrame, const Transform& rbBFrame) : JointSW(_arr,2) {
|
||||
|
||||
A=rbA;
|
||||
B=rbB;
|
||||
|
||||
|
||||
m_rbAFrame=rbAFrame;
|
||||
m_rbBFrame=rbBFrame;
|
||||
|
||||
m_swingSpan1 = Math_PI/4.0;
|
||||
m_swingSpan2 = Math_PI/4.0;
|
||||
m_twistSpan = Math_PI*2;
|
||||
m_biasFactor = 0.3f;
|
||||
m_relaxationFactor = 1.0f;
|
||||
|
||||
m_solveTwistLimit = false;
|
||||
m_solveSwingLimit = false;
|
||||
|
||||
A->add_constraint(this,0);
|
||||
B->add_constraint(this,1);
|
||||
|
||||
m_appliedImpulse=0;
|
||||
}
|
||||
|
||||
|
||||
bool ConeTwistJointSW::setup(float p_step) {
|
||||
m_appliedImpulse = real_t(0.);
|
||||
|
||||
//set bias, sign, clear accumulator
|
||||
m_swingCorrection = real_t(0.);
|
||||
m_twistLimitSign = real_t(0.);
|
||||
m_solveTwistLimit = false;
|
||||
m_solveSwingLimit = false;
|
||||
m_accTwistLimitImpulse = real_t(0.);
|
||||
m_accSwingLimitImpulse = real_t(0.);
|
||||
|
||||
if (!m_angularOnly)
|
||||
{
|
||||
Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
|
||||
Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
|
||||
Vector3 relPos = pivotBInW - pivotAInW;
|
||||
|
||||
Vector3 normal[3];
|
||||
if (relPos.length_squared() > CMP_EPSILON)
|
||||
{
|
||||
normal[0] = relPos.normalized();
|
||||
}
|
||||
else
|
||||
{
|
||||
normal[0]=Vector3(real_t(1.0),0,0);
|
||||
}
|
||||
|
||||
plane_space(normal[0], normal[1], normal[2]);
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
memnew_placement(&m_jac[i], JacobianEntrySW(
|
||||
A->get_transform().basis.transposed(),
|
||||
B->get_transform().basis.transposed(),
|
||||
pivotAInW - A->get_transform().origin,
|
||||
pivotBInW - B->get_transform().origin,
|
||||
normal[i],
|
||||
A->get_inv_inertia(),
|
||||
A->get_inv_mass(),
|
||||
B->get_inv_inertia(),
|
||||
B->get_inv_mass()));
|
||||
}
|
||||
}
|
||||
|
||||
Vector3 b1Axis1,b1Axis2,b1Axis3;
|
||||
Vector3 b2Axis1,b2Axis2;
|
||||
|
||||
b1Axis1 = A->get_transform().basis.xform( this->m_rbAFrame.basis.get_axis(0) );
|
||||
b2Axis1 = B->get_transform().basis.xform( this->m_rbBFrame.basis.get_axis(0) );
|
||||
|
||||
real_t swing1=real_t(0.),swing2 = real_t(0.);
|
||||
|
||||
real_t swx=real_t(0.),swy = real_t(0.);
|
||||
real_t thresh = real_t(10.);
|
||||
real_t fact;
|
||||
|
||||
// Get Frame into world space
|
||||
if (m_swingSpan1 >= real_t(0.05f))
|
||||
{
|
||||
b1Axis2 = A->get_transform().basis.xform( this->m_rbAFrame.basis.get_axis(1) );
|
||||
// swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
|
||||
swx = b2Axis1.dot(b1Axis1);
|
||||
swy = b2Axis1.dot(b1Axis2);
|
||||
swing1 = atan2fast(swy, swx);
|
||||
fact = (swy*swy + swx*swx) * thresh * thresh;
|
||||
fact = fact / (fact + real_t(1.0));
|
||||
swing1 *= fact;
|
||||
|
||||
}
|
||||
|
||||
if (m_swingSpan2 >= real_t(0.05f))
|
||||
{
|
||||
b1Axis3 = A->get_transform().basis.xform( this->m_rbAFrame.basis.get_axis(2) );
|
||||
// swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
|
||||
swx = b2Axis1.dot(b1Axis1);
|
||||
swy = b2Axis1.dot(b1Axis3);
|
||||
swing2 = atan2fast(swy, swx);
|
||||
fact = (swy*swy + swx*swx) * thresh * thresh;
|
||||
fact = fact / (fact + real_t(1.0));
|
||||
swing2 *= fact;
|
||||
}
|
||||
|
||||
real_t RMaxAngle1Sq = 1.0f / (m_swingSpan1*m_swingSpan1);
|
||||
real_t RMaxAngle2Sq = 1.0f / (m_swingSpan2*m_swingSpan2);
|
||||
real_t EllipseAngle = Math::abs(swing1*swing1)* RMaxAngle1Sq + Math::abs(swing2*swing2) * RMaxAngle2Sq;
|
||||
|
||||
if (EllipseAngle > 1.0f)
|
||||
{
|
||||
m_swingCorrection = EllipseAngle-1.0f;
|
||||
m_solveSwingLimit = true;
|
||||
|
||||
// Calculate necessary axis & factors
|
||||
m_swingAxis = b2Axis1.cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.dot(b1Axis3));
|
||||
m_swingAxis.normalize();
|
||||
|
||||
real_t swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
|
||||
m_swingAxis *= swingAxisSign;
|
||||
|
||||
m_kSwing = real_t(1.) / (A->compute_angular_impulse_denominator(m_swingAxis) +
|
||||
B->compute_angular_impulse_denominator(m_swingAxis));
|
||||
|
||||
}
|
||||
|
||||
// Twist limits
|
||||
if (m_twistSpan >= real_t(0.))
|
||||
{
|
||||
Vector3 b2Axis2 = B->get_transform().basis.xform( this->m_rbBFrame.basis.get_axis(1) );
|
||||
Quat rotationArc = Quat(b2Axis1,b1Axis1);
|
||||
Vector3 TwistRef = rotationArc.xform(b2Axis2);
|
||||
real_t twist = atan2fast( TwistRef.dot(b1Axis3), TwistRef.dot(b1Axis2) );
|
||||
|
||||
real_t lockedFreeFactor = (m_twistSpan > real_t(0.05f)) ? m_limitSoftness : real_t(0.);
|
||||
if (twist <= -m_twistSpan*lockedFreeFactor)
|
||||
{
|
||||
m_twistCorrection = -(twist + m_twistSpan);
|
||||
m_solveTwistLimit = true;
|
||||
|
||||
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
|
||||
m_twistAxis.normalize();
|
||||
m_twistAxis *= -1.0f;
|
||||
|
||||
m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) +
|
||||
B->compute_angular_impulse_denominator(m_twistAxis));
|
||||
|
||||
} else
|
||||
if (twist > m_twistSpan*lockedFreeFactor)
|
||||
{
|
||||
m_twistCorrection = (twist - m_twistSpan);
|
||||
m_solveTwistLimit = true;
|
||||
|
||||
m_twistAxis = (b2Axis1 + b1Axis1) * 0.5f;
|
||||
m_twistAxis.normalize();
|
||||
|
||||
m_kTwist = real_t(1.) / (A->compute_angular_impulse_denominator(m_twistAxis) +
|
||||
B->compute_angular_impulse_denominator(m_twistAxis));
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void ConeTwistJointSW::solve(real_t timeStep)
|
||||
{
|
||||
|
||||
Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
|
||||
Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
|
||||
|
||||
real_t tau = real_t(0.3);
|
||||
|
||||
//linear part
|
||||
if (!m_angularOnly)
|
||||
{
|
||||
Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
|
||||
Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
|
||||
|
||||
Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1);
|
||||
Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2);
|
||||
Vector3 vel = vel1 - vel2;
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
const Vector3& normal = m_jac[i].m_linearJointAxis;
|
||||
real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal();
|
||||
|
||||
real_t rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
//positional error (zeroth order error)
|
||||
real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
|
||||
real_t impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
|
||||
m_appliedImpulse += impulse;
|
||||
Vector3 impulse_vector = normal * impulse;
|
||||
A->apply_impulse(pivotAInW - A->get_transform().origin, impulse_vector);
|
||||
B->apply_impulse(pivotBInW - B->get_transform().origin, -impulse_vector);
|
||||
}
|
||||
}
|
||||
|
||||
{
|
||||
///solve angular part
|
||||
const Vector3& angVelA = A->get_angular_velocity();
|
||||
const Vector3& angVelB = B->get_angular_velocity();
|
||||
|
||||
// solve swing limit
|
||||
if (m_solveSwingLimit)
|
||||
{
|
||||
real_t amplitude = ((angVelB - angVelA).dot( m_swingAxis )*m_relaxationFactor*m_relaxationFactor + m_swingCorrection*(real_t(1.)/timeStep)*m_biasFactor);
|
||||
real_t impulseMag = amplitude * m_kSwing;
|
||||
|
||||
// Clamp the accumulated impulse
|
||||
real_t temp = m_accSwingLimitImpulse;
|
||||
m_accSwingLimitImpulse = MAX(m_accSwingLimitImpulse + impulseMag, real_t(0.0) );
|
||||
impulseMag = m_accSwingLimitImpulse - temp;
|
||||
|
||||
Vector3 impulse = m_swingAxis * impulseMag;
|
||||
|
||||
A->apply_torque_impulse(impulse);
|
||||
B->apply_torque_impulse(-impulse);
|
||||
|
||||
}
|
||||
|
||||
// solve twist limit
|
||||
if (m_solveTwistLimit)
|
||||
{
|
||||
real_t amplitude = ((angVelB - angVelA).dot( m_twistAxis )*m_relaxationFactor*m_relaxationFactor + m_twistCorrection*(real_t(1.)/timeStep)*m_biasFactor );
|
||||
real_t impulseMag = amplitude * m_kTwist;
|
||||
|
||||
// Clamp the accumulated impulse
|
||||
real_t temp = m_accTwistLimitImpulse;
|
||||
m_accTwistLimitImpulse = MAX(m_accTwistLimitImpulse + impulseMag, real_t(0.0) );
|
||||
impulseMag = m_accTwistLimitImpulse - temp;
|
||||
|
||||
Vector3 impulse = m_twistAxis * impulseMag;
|
||||
|
||||
A->apply_torque_impulse(impulse);
|
||||
B->apply_torque_impulse(-impulse);
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
void ConeTwistJointSW::set_param(PhysicsServer::ConeTwistJointParam p_param, float p_value) {
|
||||
|
||||
switch(p_param) {
|
||||
case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: {
|
||||
|
||||
m_swingSpan1=p_value;
|
||||
m_swingSpan2=p_value;
|
||||
} break;
|
||||
case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN: {
|
||||
|
||||
m_twistSpan=p_value;
|
||||
} break;
|
||||
case PhysicsServer::CONE_TWIST_JOINT_BIAS: {
|
||||
|
||||
m_biasFactor=p_value;
|
||||
} break;
|
||||
case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS: {
|
||||
|
||||
m_limitSoftness=p_value;
|
||||
} break;
|
||||
case PhysicsServer::CONE_TWIST_JOINT_RELAXATION: {
|
||||
|
||||
m_relaxationFactor=p_value;
|
||||
} break;
|
||||
}
|
||||
}
|
||||
|
||||
float ConeTwistJointSW::get_param(PhysicsServer::ConeTwistJointParam p_param) const{
|
||||
|
||||
switch(p_param) {
|
||||
case PhysicsServer::CONE_TWIST_JOINT_SWING_SPAN: {
|
||||
|
||||
return m_swingSpan1;
|
||||
} break;
|
||||
case PhysicsServer::CONE_TWIST_JOINT_TWIST_SPAN: {
|
||||
|
||||
return m_twistSpan;
|
||||
} break;
|
||||
case PhysicsServer::CONE_TWIST_JOINT_BIAS: {
|
||||
|
||||
return m_biasFactor;
|
||||
} break;
|
||||
case PhysicsServer::CONE_TWIST_JOINT_SOFTNESS: {
|
||||
|
||||
return m_limitSoftness;
|
||||
} break;
|
||||
case PhysicsServer::CONE_TWIST_JOINT_RELAXATION: {
|
||||
|
||||
return m_relaxationFactor;
|
||||
} break;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
125
servers/physics/joints/cone_twist_joint_sw.h
Normal file
125
servers/physics/joints/cone_twist_joint_sw.h
Normal file
@ -0,0 +1,125 @@
|
||||
#ifndef CONE_TWIST_JOINT_SW_H
|
||||
#define CONE_TWIST_JOINT_SW_H
|
||||
|
||||
#include "servers/physics/joints_sw.h"
|
||||
#include "servers/physics/joints/jacobian_entry_sw.h"
|
||||
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
ConeTwistJointSW 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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
|
||||
Written by: Marcus Hennix
|
||||
*/
|
||||
|
||||
|
||||
|
||||
///ConeTwistJointSW can be used to simulate ragdoll joints (upper arm, leg etc)
|
||||
class ConeTwistJointSW : public JointSW
|
||||
{
|
||||
#ifdef IN_PARALLELL_SOLVER
|
||||
public:
|
||||
#endif
|
||||
|
||||
union {
|
||||
struct {
|
||||
BodySW *A;
|
||||
BodySW *B;
|
||||
};
|
||||
|
||||
BodySW *_arr[2];
|
||||
};
|
||||
|
||||
JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints
|
||||
|
||||
|
||||
real_t m_appliedImpulse;
|
||||
Transform m_rbAFrame;
|
||||
Transform m_rbBFrame;
|
||||
|
||||
real_t m_limitSoftness;
|
||||
real_t m_biasFactor;
|
||||
real_t m_relaxationFactor;
|
||||
|
||||
real_t m_swingSpan1;
|
||||
real_t m_swingSpan2;
|
||||
real_t m_twistSpan;
|
||||
|
||||
Vector3 m_swingAxis;
|
||||
Vector3 m_twistAxis;
|
||||
|
||||
real_t m_kSwing;
|
||||
real_t m_kTwist;
|
||||
|
||||
real_t m_twistLimitSign;
|
||||
real_t m_swingCorrection;
|
||||
real_t m_twistCorrection;
|
||||
|
||||
real_t m_accSwingLimitImpulse;
|
||||
real_t m_accTwistLimitImpulse;
|
||||
|
||||
bool m_angularOnly;
|
||||
bool m_solveTwistLimit;
|
||||
bool m_solveSwingLimit;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_CONE_TWIST; }
|
||||
|
||||
virtual bool setup(float p_step);
|
||||
virtual void solve(float p_step);
|
||||
|
||||
ConeTwistJointSW(BodySW* rbA,BodySW* rbB,const Transform& rbAFrame, const Transform& rbBFrame);
|
||||
|
||||
|
||||
void setAngularOnly(bool angularOnly)
|
||||
{
|
||||
m_angularOnly = angularOnly;
|
||||
}
|
||||
|
||||
void setLimit(real_t _swingSpan1,real_t _swingSpan2,real_t _twistSpan, real_t _softness = 0.8f, real_t _biasFactor = 0.3f, real_t _relaxationFactor = 1.0f)
|
||||
{
|
||||
m_swingSpan1 = _swingSpan1;
|
||||
m_swingSpan2 = _swingSpan2;
|
||||
m_twistSpan = _twistSpan;
|
||||
|
||||
m_limitSoftness = _softness;
|
||||
m_biasFactor = _biasFactor;
|
||||
m_relaxationFactor = _relaxationFactor;
|
||||
}
|
||||
|
||||
inline int getSolveTwistLimit()
|
||||
{
|
||||
return m_solveTwistLimit;
|
||||
}
|
||||
|
||||
inline int getSolveSwingLimit()
|
||||
{
|
||||
return m_solveTwistLimit;
|
||||
}
|
||||
|
||||
inline real_t getTwistLimitSign()
|
||||
{
|
||||
return m_twistLimitSign;
|
||||
}
|
||||
|
||||
void set_param(PhysicsServer::ConeTwistJointParam p_param, float p_value);
|
||||
float get_param(PhysicsServer::ConeTwistJointParam p_param) const;
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif // CONE_TWIST_JOINT_SW_H
|
||||
691
servers/physics/joints/generic_6dof_joint_sw.cpp
Normal file
691
servers/physics/joints/generic_6dof_joint_sw.cpp
Normal file
@ -0,0 +1,691 @@
|
||||
#include "generic_6dof_joint_sw.h"
|
||||
|
||||
|
||||
|
||||
#define GENERIC_D6_DISABLE_WARMSTARTING 1
|
||||
|
||||
real_t btGetMatrixElem(const Matrix3& mat, int index);
|
||||
real_t btGetMatrixElem(const Matrix3& mat, int index)
|
||||
{
|
||||
int i = index%3;
|
||||
int j = index/3;
|
||||
return mat[i][j];
|
||||
}
|
||||
|
||||
///MatrixToEulerXYZ from http://www.geometrictools.com/LibFoundation/Mathematics/Wm4Matrix3.inl.html
|
||||
bool matrixToEulerXYZ(const Matrix3& mat,Vector3& xyz);
|
||||
bool matrixToEulerXYZ(const Matrix3& mat,Vector3& xyz)
|
||||
{
|
||||
// // rot = cy*cz -cy*sz sy
|
||||
// // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx
|
||||
// // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy
|
||||
//
|
||||
|
||||
if (btGetMatrixElem(mat,2) < real_t(1.0))
|
||||
{
|
||||
if (btGetMatrixElem(mat,2) > real_t(-1.0))
|
||||
{
|
||||
xyz[0] = Math::atan2(-btGetMatrixElem(mat,5),btGetMatrixElem(mat,8));
|
||||
xyz[1] = Math::asin(btGetMatrixElem(mat,2));
|
||||
xyz[2] = Math::atan2(-btGetMatrixElem(mat,1),btGetMatrixElem(mat,0));
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
// WARNING. Not unique. XA - ZA = -atan2(r10,r11)
|
||||
xyz[0] = -Math::atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
|
||||
xyz[1] = -Math_PI*0.5;
|
||||
xyz[2] = real_t(0.0);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
// WARNING. Not unique. XAngle + ZAngle = atan2(r10,r11)
|
||||
xyz[0] = Math::atan2(btGetMatrixElem(mat,3),btGetMatrixElem(mat,4));
|
||||
xyz[1] = Math_PI*0.5;
|
||||
xyz[2] = 0.0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//////////////////////////// G6DOFRotationalLimitMotorSW ////////////////////////////////////
|
||||
|
||||
|
||||
int G6DOFRotationalLimitMotorSW::testLimitValue(real_t test_value)
|
||||
{
|
||||
if(m_loLimit>m_hiLimit)
|
||||
{
|
||||
m_currentLimit = 0;//Free from violation
|
||||
return 0;
|
||||
}
|
||||
|
||||
if (test_value < m_loLimit)
|
||||
{
|
||||
m_currentLimit = 1;//low limit violation
|
||||
m_currentLimitError = test_value - m_loLimit;
|
||||
return 1;
|
||||
}
|
||||
else if (test_value> m_hiLimit)
|
||||
{
|
||||
m_currentLimit = 2;//High limit violation
|
||||
m_currentLimitError = test_value - m_hiLimit;
|
||||
return 2;
|
||||
};
|
||||
|
||||
m_currentLimit = 0;//Free from violation
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
real_t G6DOFRotationalLimitMotorSW::solveAngularLimits(
|
||||
real_t timeStep,Vector3& axis,real_t jacDiagABInv,
|
||||
BodySW * body0, BodySW * body1)
|
||||
{
|
||||
if (needApplyTorques()==false) return 0.0f;
|
||||
|
||||
real_t target_velocity = m_targetVelocity;
|
||||
real_t maxMotorForce = m_maxMotorForce;
|
||||
|
||||
//current error correction
|
||||
if (m_currentLimit!=0)
|
||||
{
|
||||
target_velocity = -m_ERP*m_currentLimitError/(timeStep);
|
||||
maxMotorForce = m_maxLimitForce;
|
||||
}
|
||||
|
||||
maxMotorForce *= timeStep;
|
||||
|
||||
// current velocity difference
|
||||
Vector3 vel_diff = body0->get_angular_velocity();
|
||||
if (body1)
|
||||
{
|
||||
vel_diff -= body1->get_angular_velocity();
|
||||
}
|
||||
|
||||
|
||||
|
||||
real_t rel_vel = axis.dot(vel_diff);
|
||||
|
||||
// correction velocity
|
||||
real_t motor_relvel = m_limitSoftness*(target_velocity - m_damping*rel_vel);
|
||||
|
||||
|
||||
if ( motor_relvel < CMP_EPSILON && motor_relvel > -CMP_EPSILON )
|
||||
{
|
||||
return 0.0f;//no need for applying force
|
||||
}
|
||||
|
||||
|
||||
// correction impulse
|
||||
real_t unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;
|
||||
|
||||
// clip correction impulse
|
||||
real_t clippedMotorImpulse;
|
||||
|
||||
///@todo: should clip against accumulated impulse
|
||||
if (unclippedMotorImpulse>0.0f)
|
||||
{
|
||||
clippedMotorImpulse = unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
|
||||
}
|
||||
else
|
||||
{
|
||||
clippedMotorImpulse = unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
|
||||
}
|
||||
|
||||
|
||||
// sort with accumulated impulses
|
||||
real_t lo = real_t(-1e30);
|
||||
real_t hi = real_t(1e30);
|
||||
|
||||
real_t oldaccumImpulse = m_accumulatedImpulse;
|
||||
real_t sum = oldaccumImpulse + clippedMotorImpulse;
|
||||
m_accumulatedImpulse = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
|
||||
|
||||
clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;
|
||||
|
||||
|
||||
|
||||
Vector3 motorImp = clippedMotorImpulse * axis;
|
||||
|
||||
|
||||
body0->apply_torque_impulse(motorImp);
|
||||
if (body1) body1->apply_torque_impulse(-motorImp);
|
||||
|
||||
return clippedMotorImpulse;
|
||||
|
||||
|
||||
}
|
||||
|
||||
//////////////////////////// End G6DOFRotationalLimitMotorSW ////////////////////////////////////
|
||||
|
||||
//////////////////////////// G6DOFTranslationalLimitMotorSW ////////////////////////////////////
|
||||
real_t G6DOFTranslationalLimitMotorSW::solveLinearAxis(
|
||||
real_t timeStep,
|
||||
real_t jacDiagABInv,
|
||||
BodySW* body1,const Vector3 &pointInA,
|
||||
BodySW* body2,const Vector3 &pointInB,
|
||||
int limit_index,
|
||||
const Vector3 & axis_normal_on_a,
|
||||
const Vector3 & anchorPos)
|
||||
{
|
||||
|
||||
///find relative velocity
|
||||
// Vector3 rel_pos1 = pointInA - body1->get_transform().origin;
|
||||
// Vector3 rel_pos2 = pointInB - body2->get_transform().origin;
|
||||
Vector3 rel_pos1 = anchorPos - body1->get_transform().origin;
|
||||
Vector3 rel_pos2 = anchorPos - body2->get_transform().origin;
|
||||
|
||||
Vector3 vel1 = body1->get_velocity_in_local_point(rel_pos1);
|
||||
Vector3 vel2 = body2->get_velocity_in_local_point(rel_pos2);
|
||||
Vector3 vel = vel1 - vel2;
|
||||
|
||||
real_t rel_vel = axis_normal_on_a.dot(vel);
|
||||
|
||||
|
||||
|
||||
/// apply displacement correction
|
||||
|
||||
//positional error (zeroth order error)
|
||||
real_t depth = -(pointInA - pointInB).dot(axis_normal_on_a);
|
||||
real_t lo = real_t(-1e30);
|
||||
real_t hi = real_t(1e30);
|
||||
|
||||
real_t minLimit = m_lowerLimit[limit_index];
|
||||
real_t maxLimit = m_upperLimit[limit_index];
|
||||
|
||||
//handle the limits
|
||||
if (minLimit < maxLimit)
|
||||
{
|
||||
{
|
||||
if (depth > maxLimit)
|
||||
{
|
||||
depth -= maxLimit;
|
||||
lo = real_t(0.);
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
if (depth < minLimit)
|
||||
{
|
||||
depth -= minLimit;
|
||||
hi = real_t(0.);
|
||||
}
|
||||
else
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
real_t normalImpulse= m_limitSoftness[limit_index]*(m_restitution[limit_index]*depth/timeStep - m_damping[limit_index]*rel_vel) * jacDiagABInv;
|
||||
|
||||
|
||||
|
||||
|
||||
real_t oldNormalImpulse = m_accumulatedImpulse[limit_index];
|
||||
real_t sum = oldNormalImpulse + normalImpulse;
|
||||
m_accumulatedImpulse[limit_index] = sum > hi ? real_t(0.) : sum < lo ? real_t(0.) : sum;
|
||||
normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;
|
||||
|
||||
Vector3 impulse_vector = axis_normal_on_a * normalImpulse;
|
||||
body1->apply_impulse( rel_pos1, impulse_vector);
|
||||
body2->apply_impulse( rel_pos2, -impulse_vector);
|
||||
return normalImpulse;
|
||||
}
|
||||
|
||||
//////////////////////////// G6DOFTranslationalLimitMotorSW ////////////////////////////////////
|
||||
|
||||
|
||||
Generic6DOFJointSW::Generic6DOFJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB, bool useLinearReferenceFrameA)
|
||||
: JointSW(_arr,2)
|
||||
, m_frameInA(frameInA)
|
||||
, m_frameInB(frameInB),
|
||||
m_useLinearReferenceFrameA(useLinearReferenceFrameA)
|
||||
{
|
||||
A=rbA;
|
||||
B=rbB;
|
||||
A->add_constraint(this,0);
|
||||
B->add_constraint(this,1);
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void Generic6DOFJointSW::calculateAngleInfo()
|
||||
{
|
||||
Matrix3 relative_frame = m_calculatedTransformA.basis.inverse()*m_calculatedTransformB.basis;
|
||||
|
||||
matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
|
||||
|
||||
|
||||
|
||||
// in euler angle mode we do not actually constrain the angular velocity
|
||||
// along the axes axis[0] and axis[2] (although we do use axis[1]) :
|
||||
//
|
||||
// to get constrain w2-w1 along ...not
|
||||
// ------ --------------------- ------
|
||||
// d(angle[0])/dt = 0 ax[1] x ax[2] ax[0]
|
||||
// d(angle[1])/dt = 0 ax[1]
|
||||
// d(angle[2])/dt = 0 ax[0] x ax[1] ax[2]
|
||||
//
|
||||
// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0.
|
||||
// to prove the result for angle[0], write the expression for angle[0] from
|
||||
// GetInfo1 then take the derivative. to prove this for angle[2] it is
|
||||
// easier to take the euler rate expression for d(angle[2])/dt with respect
|
||||
// to the components of w and set that to 0.
|
||||
|
||||
Vector3 axis0 = m_calculatedTransformB.basis.get_axis(0);
|
||||
Vector3 axis2 = m_calculatedTransformA.basis.get_axis(2);
|
||||
|
||||
m_calculatedAxis[1] = axis2.cross(axis0);
|
||||
m_calculatedAxis[0] = m_calculatedAxis[1].cross(axis2);
|
||||
m_calculatedAxis[2] = axis0.cross(m_calculatedAxis[1]);
|
||||
|
||||
|
||||
// if(m_debugDrawer)
|
||||
// {
|
||||
//
|
||||
// char buff[300];
|
||||
// sprintf(buff,"\n X: %.2f ; Y: %.2f ; Z: %.2f ",
|
||||
// m_calculatedAxisAngleDiff[0],
|
||||
// m_calculatedAxisAngleDiff[1],
|
||||
// m_calculatedAxisAngleDiff[2]);
|
||||
// m_debugDrawer->reportErrorWarning(buff);
|
||||
// }
|
||||
|
||||
}
|
||||
|
||||
void Generic6DOFJointSW::calculateTransforms()
|
||||
{
|
||||
m_calculatedTransformA = A->get_transform() * m_frameInA;
|
||||
m_calculatedTransformB = B->get_transform() * m_frameInB;
|
||||
|
||||
calculateAngleInfo();
|
||||
}
|
||||
|
||||
|
||||
void Generic6DOFJointSW::buildLinearJacobian(
|
||||
JacobianEntrySW & jacLinear,const Vector3 & normalWorld,
|
||||
const Vector3 & pivotAInW,const Vector3 & pivotBInW)
|
||||
{
|
||||
memnew_placement(&jacLinear, JacobianEntrySW(
|
||||
A->get_transform().basis.transposed(),
|
||||
B->get_transform().basis.transposed(),
|
||||
pivotAInW - A->get_transform().origin,
|
||||
pivotBInW - B->get_transform().origin,
|
||||
normalWorld,
|
||||
A->get_inv_inertia(),
|
||||
A->get_inv_mass(),
|
||||
B->get_inv_inertia(),
|
||||
B->get_inv_mass()));
|
||||
|
||||
}
|
||||
|
||||
void Generic6DOFJointSW::buildAngularJacobian(
|
||||
JacobianEntrySW & jacAngular,const Vector3 & jointAxisW)
|
||||
{
|
||||
memnew_placement(&jacAngular, JacobianEntrySW(jointAxisW,
|
||||
A->get_transform().basis.transposed(),
|
||||
B->get_transform().basis.transposed(),
|
||||
A->get_inv_inertia(),
|
||||
B->get_inv_inertia()));
|
||||
|
||||
}
|
||||
|
||||
bool Generic6DOFJointSW::testAngularLimitMotor(int axis_index)
|
||||
{
|
||||
real_t angle = m_calculatedAxisAngleDiff[axis_index];
|
||||
|
||||
//test limits
|
||||
m_angularLimits[axis_index].testLimitValue(angle);
|
||||
return m_angularLimits[axis_index].needApplyTorques();
|
||||
}
|
||||
|
||||
bool Generic6DOFJointSW::setup(float p_step) {
|
||||
|
||||
// Clear accumulated impulses for the next simulation step
|
||||
m_linearLimits.m_accumulatedImpulse=Vector3(real_t(0.), real_t(0.), real_t(0.));
|
||||
int i;
|
||||
for(i = 0; i < 3; i++)
|
||||
{
|
||||
m_angularLimits[i].m_accumulatedImpulse = real_t(0.);
|
||||
}
|
||||
//calculates transform
|
||||
calculateTransforms();
|
||||
|
||||
// const Vector3& pivotAInW = m_calculatedTransformA.origin;
|
||||
// const Vector3& pivotBInW = m_calculatedTransformB.origin;
|
||||
calcAnchorPos();
|
||||
Vector3 pivotAInW = m_AnchorPos;
|
||||
Vector3 pivotBInW = m_AnchorPos;
|
||||
|
||||
// not used here
|
||||
// Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
|
||||
// Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
|
||||
|
||||
Vector3 normalWorld;
|
||||
//linear part
|
||||
for (i=0;i<3;i++)
|
||||
{
|
||||
if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i))
|
||||
{
|
||||
if (m_useLinearReferenceFrameA)
|
||||
normalWorld = m_calculatedTransformA.basis.get_axis(i);
|
||||
else
|
||||
normalWorld = m_calculatedTransformB.basis.get_axis(i);
|
||||
|
||||
buildLinearJacobian(
|
||||
m_jacLinear[i],normalWorld ,
|
||||
pivotAInW,pivotBInW);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// angular part
|
||||
for (i=0;i<3;i++)
|
||||
{
|
||||
//calculates error angle
|
||||
if (m_angularLimits[i].m_enableLimit && testAngularLimitMotor(i))
|
||||
{
|
||||
normalWorld = this->getAxis(i);
|
||||
// Create angular atom
|
||||
buildAngularJacobian(m_jacAng[i],normalWorld);
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
void Generic6DOFJointSW::solve(real_t timeStep)
|
||||
{
|
||||
m_timeStep = timeStep;
|
||||
|
||||
//calculateTransforms();
|
||||
|
||||
int i;
|
||||
|
||||
// linear
|
||||
|
||||
Vector3 pointInA = m_calculatedTransformA.origin;
|
||||
Vector3 pointInB = m_calculatedTransformB.origin;
|
||||
|
||||
real_t jacDiagABInv;
|
||||
Vector3 linear_axis;
|
||||
for (i=0;i<3;i++)
|
||||
{
|
||||
if (m_linearLimits.enable_limit[i] && m_linearLimits.isLimited(i))
|
||||
{
|
||||
jacDiagABInv = real_t(1.) / m_jacLinear[i].getDiagonal();
|
||||
|
||||
if (m_useLinearReferenceFrameA)
|
||||
linear_axis = m_calculatedTransformA.basis.get_axis(i);
|
||||
else
|
||||
linear_axis = m_calculatedTransformB.basis.get_axis(i);
|
||||
|
||||
m_linearLimits.solveLinearAxis(
|
||||
m_timeStep,
|
||||
jacDiagABInv,
|
||||
A,pointInA,
|
||||
B,pointInB,
|
||||
i,linear_axis, m_AnchorPos);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// angular
|
||||
Vector3 angular_axis;
|
||||
real_t angularJacDiagABInv;
|
||||
for (i=0;i<3;i++)
|
||||
{
|
||||
if (m_angularLimits[i].m_enableLimit && m_angularLimits[i].needApplyTorques())
|
||||
{
|
||||
|
||||
// get axis
|
||||
angular_axis = getAxis(i);
|
||||
|
||||
angularJacDiagABInv = real_t(1.) / m_jacAng[i].getDiagonal();
|
||||
|
||||
m_angularLimits[i].solveAngularLimits(m_timeStep,angular_axis,angularJacDiagABInv, A,B);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Generic6DOFJointSW::updateRHS(real_t timeStep)
|
||||
{
|
||||
(void)timeStep;
|
||||
|
||||
}
|
||||
|
||||
Vector3 Generic6DOFJointSW::getAxis(int axis_index) const
|
||||
{
|
||||
return m_calculatedAxis[axis_index];
|
||||
}
|
||||
|
||||
real_t Generic6DOFJointSW::getAngle(int axis_index) const
|
||||
{
|
||||
return m_calculatedAxisAngleDiff[axis_index];
|
||||
}
|
||||
|
||||
void Generic6DOFJointSW::calcAnchorPos(void)
|
||||
{
|
||||
real_t imA = A->get_inv_mass();
|
||||
real_t imB = B->get_inv_mass();
|
||||
real_t weight;
|
||||
if(imB == real_t(0.0))
|
||||
{
|
||||
weight = real_t(1.0);
|
||||
}
|
||||
else
|
||||
{
|
||||
weight = imA / (imA + imB);
|
||||
}
|
||||
const Vector3& pA = m_calculatedTransformA.origin;
|
||||
const Vector3& pB = m_calculatedTransformB.origin;
|
||||
m_AnchorPos = pA * weight + pB * (real_t(1.0) - weight);
|
||||
return;
|
||||
} // Generic6DOFJointSW::calcAnchorPos()
|
||||
|
||||
|
||||
void Generic6DOFJointSW::set_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param, float p_value) {
|
||||
|
||||
ERR_FAIL_INDEX(p_axis,3);
|
||||
switch(p_param) {
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
|
||||
|
||||
m_linearLimits.m_lowerLimit[p_axis]=p_value;
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {
|
||||
|
||||
m_linearLimits.m_upperLimit[p_axis]=p_value;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {
|
||||
|
||||
m_linearLimits.m_limitSoftness[p_axis]=p_value;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: {
|
||||
|
||||
m_linearLimits.m_restitution[p_axis]=p_value;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: {
|
||||
|
||||
m_linearLimits.m_damping[p_axis]=p_value;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {
|
||||
|
||||
m_angularLimits[p_axis].m_loLimit=p_value;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {
|
||||
|
||||
m_angularLimits[p_axis].m_hiLimit=p_value;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {
|
||||
|
||||
m_angularLimits[p_axis].m_limitSoftness;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: {
|
||||
|
||||
m_angularLimits[p_axis].m_damping=p_value;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: {
|
||||
|
||||
m_angularLimits[p_axis].m_bounce=p_value;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {
|
||||
|
||||
m_angularLimits[p_axis].m_maxLimitForce=p_value;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: {
|
||||
|
||||
m_angularLimits[p_axis].m_ERP=p_value;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {
|
||||
|
||||
m_angularLimits[p_axis].m_targetVelocity=p_value;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {
|
||||
|
||||
m_angularLimits[p_axis].m_maxLimitForce=p_value;
|
||||
|
||||
} break;
|
||||
}
|
||||
}
|
||||
|
||||
float Generic6DOFJointSW::get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param) const{
|
||||
ERR_FAIL_INDEX_V(p_axis,3,0);
|
||||
switch(p_param) {
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_LOWER_LIMIT: {
|
||||
|
||||
return m_linearLimits.m_lowerLimit[p_axis];
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_UPPER_LIMIT: {
|
||||
|
||||
return m_linearLimits.m_upperLimit[p_axis];
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS: {
|
||||
|
||||
return m_linearLimits.m_limitSoftness[p_axis];
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_RESTITUTION: {
|
||||
|
||||
return m_linearLimits.m_restitution[p_axis];
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_LINEAR_DAMPING: {
|
||||
|
||||
return m_linearLimits.m_damping[p_axis];
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_LOWER_LIMIT: {
|
||||
|
||||
return m_angularLimits[p_axis].m_loLimit;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_UPPER_LIMIT: {
|
||||
|
||||
return m_angularLimits[p_axis].m_hiLimit;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS: {
|
||||
|
||||
return m_angularLimits[p_axis].m_limitSoftness;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_DAMPING: {
|
||||
|
||||
return m_angularLimits[p_axis].m_damping;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_RESTITUTION: {
|
||||
|
||||
return m_angularLimits[p_axis].m_bounce;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_FORCE_LIMIT: {
|
||||
|
||||
return m_angularLimits[p_axis].m_maxLimitForce;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_ERP: {
|
||||
|
||||
return m_angularLimits[p_axis].m_ERP;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY: {
|
||||
|
||||
return m_angularLimits[p_axis].m_targetVelocity;
|
||||
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT: {
|
||||
|
||||
return m_angularLimits[p_axis].m_maxLimitForce;
|
||||
|
||||
} break;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
void Generic6DOFJointSW::set_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value){
|
||||
|
||||
ERR_FAIL_INDEX(p_axis,3);
|
||||
|
||||
switch(p_flag) {
|
||||
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
|
||||
|
||||
m_linearLimits.enable_limit[p_axis]=p_value;
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
|
||||
|
||||
m_angularLimits[p_axis].m_enableLimit=p_value;
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {
|
||||
|
||||
m_angularLimits[p_axis].m_enableMotor=p_value;
|
||||
} break;
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
bool Generic6DOFJointSW::get_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag) const{
|
||||
|
||||
ERR_FAIL_INDEX_V(p_axis,3,0);
|
||||
switch(p_flag) {
|
||||
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT: {
|
||||
|
||||
return m_linearLimits.enable_limit[p_axis];
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT: {
|
||||
|
||||
return m_angularLimits[p_axis].m_enableLimit;
|
||||
} break;
|
||||
case PhysicsServer::G6DOF_JOINT_FLAG_ENABLE_MOTOR: {
|
||||
|
||||
return m_angularLimits[p_axis].m_enableMotor;
|
||||
} break;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
428
servers/physics/joints/generic_6dof_joint_sw.h
Normal file
428
servers/physics/joints/generic_6dof_joint_sw.h
Normal file
@ -0,0 +1,428 @@
|
||||
#ifndef GENERIC_6DOF_JOINT_SW_H
|
||||
#define GENERIC_6DOF_JOINT_SW_H
|
||||
|
||||
#include "servers/physics/joints_sw.h"
|
||||
#include "servers/physics/joints/jacobian_entry_sw.h"
|
||||
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
/*
|
||||
2007-09-09
|
||||
Generic6DOFJointSW Refactored by Francisco Le?n
|
||||
email: projectileman@yahoo.com
|
||||
http://gimpact.sf.net
|
||||
*/
|
||||
|
||||
|
||||
//! Rotation Limit structure for generic joints
|
||||
class G6DOFRotationalLimitMotorSW {
|
||||
public:
|
||||
//! limit_parameters
|
||||
//!@{
|
||||
real_t m_loLimit;//!< joint limit
|
||||
real_t m_hiLimit;//!< joint limit
|
||||
real_t m_targetVelocity;//!< target motor velocity
|
||||
real_t m_maxMotorForce;//!< max force on motor
|
||||
real_t m_maxLimitForce;//!< max force on limit
|
||||
real_t m_damping;//!< Damping.
|
||||
real_t m_limitSoftness;//! Relaxation factor
|
||||
real_t m_ERP;//!< Error tolerance factor when joint is at limit
|
||||
real_t m_bounce;//!< restitution factor
|
||||
bool m_enableMotor;
|
||||
bool m_enableLimit;
|
||||
|
||||
//!@}
|
||||
|
||||
//! temp_variables
|
||||
//!@{
|
||||
real_t m_currentLimitError;//! How much is violated this limit
|
||||
int m_currentLimit;//!< 0=free, 1=at lo limit, 2=at hi limit
|
||||
real_t m_accumulatedImpulse;
|
||||
//!@}
|
||||
|
||||
G6DOFRotationalLimitMotorSW()
|
||||
{
|
||||
m_accumulatedImpulse = 0.f;
|
||||
m_targetVelocity = 0;
|
||||
m_maxMotorForce = 0.1f;
|
||||
m_maxLimitForce = 300.0f;
|
||||
m_loLimit = -1e30;
|
||||
m_hiLimit = 1e30;
|
||||
m_ERP = 0.5f;
|
||||
m_bounce = 0.0f;
|
||||
m_damping = 1.0f;
|
||||
m_limitSoftness = 0.5f;
|
||||
m_currentLimit = 0;
|
||||
m_currentLimitError = 0;
|
||||
m_enableMotor = false;
|
||||
m_enableLimit=false;
|
||||
}
|
||||
|
||||
G6DOFRotationalLimitMotorSW(const G6DOFRotationalLimitMotorSW & limot)
|
||||
{
|
||||
m_targetVelocity = limot.m_targetVelocity;
|
||||
m_maxMotorForce = limot.m_maxMotorForce;
|
||||
m_limitSoftness = limot.m_limitSoftness;
|
||||
m_loLimit = limot.m_loLimit;
|
||||
m_hiLimit = limot.m_hiLimit;
|
||||
m_ERP = limot.m_ERP;
|
||||
m_bounce = limot.m_bounce;
|
||||
m_currentLimit = limot.m_currentLimit;
|
||||
m_currentLimitError = limot.m_currentLimitError;
|
||||
m_enableMotor = limot.m_enableMotor;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//! Is limited
|
||||
bool isLimited()
|
||||
{
|
||||
if(m_loLimit>=m_hiLimit) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
//! Need apply correction
|
||||
bool needApplyTorques()
|
||||
{
|
||||
if(m_currentLimit == 0 && m_enableMotor == false) return false;
|
||||
return true;
|
||||
}
|
||||
|
||||
//! calculates error
|
||||
/*!
|
||||
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,BodySW * body0, BodySW * body1);
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
class G6DOFTranslationalLimitMotorSW
|
||||
{
|
||||
public:
|
||||
Vector3 m_lowerLimit;//!< the constraint lower limits
|
||||
Vector3 m_upperLimit;//!< the constraint upper limits
|
||||
Vector3 m_accumulatedImpulse;
|
||||
//! Linear_Limit_parameters
|
||||
//!@{
|
||||
Vector3 m_limitSoftness;//!< Softness for linear limit
|
||||
Vector3 m_damping;//!< Damping for linear limit
|
||||
Vector3 m_restitution;//! Bounce parameter for linear limit
|
||||
//!@}
|
||||
bool enable_limit[3];
|
||||
|
||||
G6DOFTranslationalLimitMotorSW()
|
||||
{
|
||||
m_lowerLimit=Vector3(0.f,0.f,0.f);
|
||||
m_upperLimit=Vector3(0.f,0.f,0.f);
|
||||
m_accumulatedImpulse=Vector3(0.f,0.f,0.f);
|
||||
|
||||
m_limitSoftness = Vector3(1,1,1)*0.7f;
|
||||
m_damping = Vector3(1,1,1)*real_t(1.0f);
|
||||
m_restitution = Vector3(1,1,1)*real_t(0.5f);
|
||||
|
||||
enable_limit[0]=true;
|
||||
enable_limit[1]=true;
|
||||
enable_limit[2]=true;
|
||||
}
|
||||
|
||||
G6DOFTranslationalLimitMotorSW(const G6DOFTranslationalLimitMotorSW & other )
|
||||
{
|
||||
m_lowerLimit = other.m_lowerLimit;
|
||||
m_upperLimit = other.m_upperLimit;
|
||||
m_accumulatedImpulse = other.m_accumulatedImpulse;
|
||||
|
||||
m_limitSoftness = other.m_limitSoftness ;
|
||||
m_damping = other.m_damping;
|
||||
m_restitution = other.m_restitution;
|
||||
}
|
||||
|
||||
//! Test limit
|
||||
/*!
|
||||
- free means upper < lower,
|
||||
- locked means upper == lower
|
||||
- limited means upper > lower
|
||||
- limitIndex: first 3 are linear, next 3 are angular
|
||||
*/
|
||||
inline bool isLimited(int limitIndex)
|
||||
{
|
||||
return (m_upperLimit[limitIndex] >= m_lowerLimit[limitIndex]);
|
||||
}
|
||||
|
||||
|
||||
real_t solveLinearAxis(
|
||||
real_t timeStep,
|
||||
real_t jacDiagABInv,
|
||||
BodySW* body1,const Vector3 &pointInA,
|
||||
BodySW* body2,const Vector3 &pointInB,
|
||||
int limit_index,
|
||||
const Vector3 & axis_normal_on_a,
|
||||
const Vector3 & anchorPos);
|
||||
|
||||
|
||||
};
|
||||
|
||||
|
||||
class Generic6DOFJointSW : public JointSW
|
||||
{
|
||||
protected:
|
||||
|
||||
|
||||
union {
|
||||
struct {
|
||||
BodySW *A;
|
||||
BodySW *B;
|
||||
};
|
||||
|
||||
BodySW *_arr[2];
|
||||
};
|
||||
|
||||
//! relative_frames
|
||||
//!@{
|
||||
Transform m_frameInA;//!< the constraint space w.r.t body A
|
||||
Transform m_frameInB;//!< the constraint space w.r.t body B
|
||||
//!@}
|
||||
|
||||
//! Jacobians
|
||||
//!@{
|
||||
JacobianEntrySW m_jacLinear[3];//!< 3 orthogonal linear constraints
|
||||
JacobianEntrySW m_jacAng[3];//!< 3 orthogonal angular constraints
|
||||
//!@}
|
||||
|
||||
//! Linear_Limit_parameters
|
||||
//!@{
|
||||
G6DOFTranslationalLimitMotorSW m_linearLimits;
|
||||
//!@}
|
||||
|
||||
|
||||
//! hinge_parameters
|
||||
//!@{
|
||||
G6DOFRotationalLimitMotorSW m_angularLimits[3];
|
||||
//!@}
|
||||
|
||||
|
||||
protected:
|
||||
//! temporal variables
|
||||
//!@{
|
||||
real_t m_timeStep;
|
||||
Transform m_calculatedTransformA;
|
||||
Transform m_calculatedTransformB;
|
||||
Vector3 m_calculatedAxisAngleDiff;
|
||||
Vector3 m_calculatedAxis[3];
|
||||
|
||||
Vector3 m_AnchorPos; // point betwen pivots of bodies A and B to solve linear axes
|
||||
|
||||
bool m_useLinearReferenceFrameA;
|
||||
|
||||
//!@}
|
||||
|
||||
Generic6DOFJointSW& operator=(Generic6DOFJointSW& other)
|
||||
{
|
||||
ERR_PRINT("pito");
|
||||
(void) other;
|
||||
return *this;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void buildLinearJacobian(
|
||||
JacobianEntrySW & jacLinear,const Vector3 & normalWorld,
|
||||
const Vector3 & pivotAInW,const Vector3 & pivotBInW);
|
||||
|
||||
void buildAngularJacobian(JacobianEntrySW & jacAngular,const Vector3 & jointAxisW);
|
||||
|
||||
|
||||
//! calcs the euler angles between the two bodies.
|
||||
void calculateAngleInfo();
|
||||
|
||||
|
||||
|
||||
public:
|
||||
Generic6DOFJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB ,bool useLinearReferenceFrameA);
|
||||
|
||||
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_6DOF; }
|
||||
|
||||
virtual bool setup(float p_step);
|
||||
virtual void solve(float p_step);
|
||||
|
||||
|
||||
//! 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
|
||||
*/
|
||||
void calculateTransforms();
|
||||
|
||||
//! Gets the global transform of the offset for body A
|
||||
/*!
|
||||
\sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo.
|
||||
*/
|
||||
const Transform & getCalculatedTransformA() const
|
||||
{
|
||||
return m_calculatedTransformA;
|
||||
}
|
||||
|
||||
//! Gets the global transform of the offset for body B
|
||||
/*!
|
||||
\sa Generic6DOFJointSW.getFrameOffsetA, Generic6DOFJointSW.getFrameOffsetB, Generic6DOFJointSW.calculateAngleInfo.
|
||||
*/
|
||||
const Transform & getCalculatedTransformB() const
|
||||
{
|
||||
return m_calculatedTransformB;
|
||||
}
|
||||
|
||||
const Transform & getFrameOffsetA() const
|
||||
{
|
||||
return m_frameInA;
|
||||
}
|
||||
|
||||
const Transform & getFrameOffsetB() const
|
||||
{
|
||||
return m_frameInB;
|
||||
}
|
||||
|
||||
|
||||
Transform & getFrameOffsetA()
|
||||
{
|
||||
return m_frameInA;
|
||||
}
|
||||
|
||||
Transform & getFrameOffsetB()
|
||||
{
|
||||
return m_frameInB;
|
||||
}
|
||||
|
||||
|
||||
//! 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.
|
||||
*/
|
||||
Vector3 getAxis(int axis_index) const;
|
||||
|
||||
//! Get the relative Euler angle
|
||||
/*!
|
||||
\pre Generic6DOFJointSW.buildJacobian must be called previously.
|
||||
*/
|
||||
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.
|
||||
*/
|
||||
bool testAngularLimitMotor(int axis_index);
|
||||
|
||||
void setLinearLowerLimit(const Vector3& linearLower)
|
||||
{
|
||||
m_linearLimits.m_lowerLimit = linearLower;
|
||||
}
|
||||
|
||||
void setLinearUpperLimit(const Vector3& linearUpper)
|
||||
{
|
||||
m_linearLimits.m_upperLimit = linearUpper;
|
||||
}
|
||||
|
||||
void setAngularLowerLimit(const Vector3& angularLower)
|
||||
{
|
||||
m_angularLimits[0].m_loLimit = angularLower.x;
|
||||
m_angularLimits[1].m_loLimit = angularLower.y;
|
||||
m_angularLimits[2].m_loLimit = angularLower.z;
|
||||
}
|
||||
|
||||
void setAngularUpperLimit(const Vector3& angularUpper)
|
||||
{
|
||||
m_angularLimits[0].m_hiLimit = angularUpper.x;
|
||||
m_angularLimits[1].m_hiLimit = angularUpper.y;
|
||||
m_angularLimits[2].m_hiLimit = angularUpper.z;
|
||||
}
|
||||
|
||||
//! Retrieves the angular limit informacion
|
||||
G6DOFRotationalLimitMotorSW * getRotationalLimitMotor(int index)
|
||||
{
|
||||
return &m_angularLimits[index];
|
||||
}
|
||||
|
||||
//! Retrieves the limit informacion
|
||||
G6DOFTranslationalLimitMotorSW * getTranslationalLimitMotor()
|
||||
{
|
||||
return &m_linearLimits;
|
||||
}
|
||||
|
||||
//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;
|
||||
m_linearLimits.m_upperLimit[axis] = hi;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_angularLimits[axis-3].m_loLimit = lo;
|
||||
m_angularLimits[axis-3].m_hiLimit = hi;
|
||||
}
|
||||
}
|
||||
|
||||
//! Test limit
|
||||
/*!
|
||||
- free means upper < lower,
|
||||
- locked means upper == lower
|
||||
- limited means upper > lower
|
||||
- limitIndex: first 3 are linear, next 3 are angular
|
||||
*/
|
||||
bool isLimited(int limitIndex)
|
||||
{
|
||||
if(limitIndex<3)
|
||||
{
|
||||
return m_linearLimits.isLimited(limitIndex);
|
||||
|
||||
}
|
||||
return m_angularLimits[limitIndex-3].isLimited();
|
||||
}
|
||||
|
||||
const BodySW* getRigidBodyA() const
|
||||
{
|
||||
return A;
|
||||
}
|
||||
const BodySW* getRigidBodyB() const
|
||||
{
|
||||
return B;
|
||||
}
|
||||
|
||||
virtual void calcAnchorPos(void); // overridable
|
||||
|
||||
void set_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param, float p_value);
|
||||
float get_param(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisParam p_param) const;
|
||||
|
||||
void set_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag, bool p_value);
|
||||
bool get_flag(Vector3::Axis p_axis,PhysicsServer::G6DOFJointAxisFlag p_flag) const;
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif // GENERIC_6DOF_JOINT_SW_H
|
||||
438
servers/physics/joints/hinge_joint_sw.cpp
Normal file
438
servers/physics/joints/hinge_joint_sw.cpp
Normal file
@ -0,0 +1,438 @@
|
||||
#include "hinge_joint_sw.h"
|
||||
|
||||
static void plane_space(const Vector3& n, Vector3& p, Vector3& q) {
|
||||
|
||||
if (Math::abs(n.z) > 0.707106781186547524400844362) {
|
||||
// choose p in y-z plane
|
||||
real_t a = n[1]*n[1] + n[2]*n[2];
|
||||
real_t k = 1.0/Math::sqrt(a);
|
||||
p=Vector3(0,-n[2]*k,n[1]*k);
|
||||
// set q = n x p
|
||||
q=Vector3(a*k,-n[0]*p[2],n[0]*p[1]);
|
||||
}
|
||||
else {
|
||||
// choose p in x-y plane
|
||||
real_t a = n.x*n.x + n.y*n.y;
|
||||
real_t k = 1.0/Math::sqrt(a);
|
||||
p=Vector3(-n.y*k,n.x*k,0);
|
||||
// set q = n x p
|
||||
q=Vector3(-n.z*p.y,n.z*p.x,a*k);
|
||||
}
|
||||
}
|
||||
|
||||
HingeJointSW::HingeJointSW(BodySW* rbA,BodySW* rbB, const Transform& frameA, const Transform& frameB) : JointSW(_arr,2) {
|
||||
|
||||
A=rbA;
|
||||
B=rbB;
|
||||
|
||||
m_rbAFrame=frameA;
|
||||
m_rbBFrame=frameB;
|
||||
// flip axis
|
||||
m_rbBFrame.basis[0][2] *= real_t(-1.);
|
||||
m_rbBFrame.basis[1][2] *= real_t(-1.);
|
||||
m_rbBFrame.basis[2][2] *= real_t(-1.);
|
||||
|
||||
|
||||
//start with free
|
||||
m_lowerLimit = Math_PI;
|
||||
m_upperLimit = -Math_PI;
|
||||
|
||||
|
||||
m_useLimit = false;
|
||||
m_biasFactor = 0.3f;
|
||||
m_relaxationFactor = 1.0f;
|
||||
m_limitSoftness = 0.9f;
|
||||
m_solveLimit = false;
|
||||
|
||||
tau=0.3;
|
||||
|
||||
m_angularOnly=false;
|
||||
m_enableAngularMotor=false;
|
||||
|
||||
A->add_constraint(this,0);
|
||||
B->add_constraint(this,1);
|
||||
|
||||
}
|
||||
|
||||
HingeJointSW::HingeJointSW(BodySW* rbA,BodySW* rbB, const Vector3& pivotInA,const Vector3& pivotInB,
|
||||
const Vector3& axisInA,const Vector3& axisInB) : JointSW(_arr,2) {
|
||||
|
||||
A=rbA;
|
||||
B=rbB;
|
||||
|
||||
m_rbAFrame.origin = pivotInA;
|
||||
|
||||
// since no frame is given, assume this to be zero angle and just pick rb transform axis
|
||||
Vector3 rbAxisA1 = rbA->get_transform().basis.get_axis(0);
|
||||
|
||||
Vector3 rbAxisA2;
|
||||
real_t projection = axisInA.dot(rbAxisA1);
|
||||
if (projection >= 1.0f - CMP_EPSILON) {
|
||||
rbAxisA1 = -rbA->get_transform().basis.get_axis(2);
|
||||
rbAxisA2 = rbA->get_transform().basis.get_axis(1);
|
||||
} else if (projection <= -1.0f + CMP_EPSILON) {
|
||||
rbAxisA1 = rbA->get_transform().basis.get_axis(2);
|
||||
rbAxisA2 = rbA->get_transform().basis.get_axis(1);
|
||||
} else {
|
||||
rbAxisA2 = axisInA.cross(rbAxisA1);
|
||||
rbAxisA1 = rbAxisA2.cross(axisInA);
|
||||
}
|
||||
|
||||
m_rbAFrame.basis=Matrix3( rbAxisA1.x,rbAxisA2.x,axisInA.x,
|
||||
rbAxisA1.y,rbAxisA2.y,axisInA.y,
|
||||
rbAxisA1.z,rbAxisA2.z,axisInA.z );
|
||||
|
||||
Quat rotationArc = Quat(axisInA,axisInB);
|
||||
Vector3 rbAxisB1 = rotationArc.xform(rbAxisA1);
|
||||
Vector3 rbAxisB2 = axisInB.cross(rbAxisB1);
|
||||
|
||||
m_rbBFrame.origin = pivotInB;
|
||||
m_rbBFrame.basis=Matrix3( rbAxisB1.x,rbAxisB2.x,-axisInB.x,
|
||||
rbAxisB1.y,rbAxisB2.y,-axisInB.y,
|
||||
rbAxisB1.z,rbAxisB2.z,-axisInB.z );
|
||||
|
||||
//start with free
|
||||
m_lowerLimit = Math_PI;
|
||||
m_upperLimit = -Math_PI;
|
||||
|
||||
|
||||
m_useLimit = false;
|
||||
m_biasFactor = 0.3f;
|
||||
m_relaxationFactor = 1.0f;
|
||||
m_limitSoftness = 0.9f;
|
||||
m_solveLimit = false;
|
||||
|
||||
tau=0.3;
|
||||
|
||||
m_angularOnly=false;
|
||||
m_enableAngularMotor=false;
|
||||
|
||||
A->add_constraint(this,0);
|
||||
B->add_constraint(this,1);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool HingeJointSW::setup(float p_step) {
|
||||
|
||||
m_appliedImpulse = real_t(0.);
|
||||
|
||||
if (!m_angularOnly)
|
||||
{
|
||||
Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
|
||||
Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
|
||||
Vector3 relPos = pivotBInW - pivotAInW;
|
||||
|
||||
Vector3 normal[3];
|
||||
if (relPos.length_squared() > CMP_EPSILON)
|
||||
{
|
||||
normal[0] = relPos.normalized();
|
||||
}
|
||||
else
|
||||
{
|
||||
normal[0]=Vector3(real_t(1.0),0,0);
|
||||
}
|
||||
|
||||
plane_space(normal[0], normal[1], normal[2]);
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
memnew_placement(&m_jac[i], JacobianEntrySW(
|
||||
A->get_transform().basis.transposed(),
|
||||
B->get_transform().basis.transposed(),
|
||||
pivotAInW - A->get_transform().origin,
|
||||
pivotBInW - B->get_transform().origin,
|
||||
normal[i],
|
||||
A->get_inv_inertia(),
|
||||
A->get_inv_mass(),
|
||||
B->get_inv_inertia(),
|
||||
B->get_inv_mass()) );
|
||||
}
|
||||
}
|
||||
|
||||
//calculate two perpendicular jointAxis, orthogonal to hingeAxis
|
||||
//these two jointAxis require equal angular velocities for both bodies
|
||||
|
||||
//this is unused for now, it's a todo
|
||||
Vector3 jointAxis0local;
|
||||
Vector3 jointAxis1local;
|
||||
|
||||
plane_space(m_rbAFrame.basis.get_axis(2),jointAxis0local,jointAxis1local);
|
||||
|
||||
A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) );
|
||||
Vector3 jointAxis0 = A->get_transform().basis.xform( jointAxis0local );
|
||||
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], JacobianEntrySW(jointAxis0,
|
||||
A->get_transform().basis.transposed(),
|
||||
B->get_transform().basis.transposed(),
|
||||
A->get_inv_inertia(),
|
||||
B->get_inv_inertia()));
|
||||
|
||||
memnew_placement(&m_jacAng[1], JacobianEntrySW(jointAxis1,
|
||||
A->get_transform().basis.transposed(),
|
||||
B->get_transform().basis.transposed(),
|
||||
A->get_inv_inertia(),
|
||||
B->get_inv_inertia()));
|
||||
|
||||
memnew_placement(&m_jacAng[2], JacobianEntrySW(hingeAxisWorld,
|
||||
A->get_transform().basis.transposed(),
|
||||
B->get_transform().basis.transposed(),
|
||||
A->get_inv_inertia(),
|
||||
B->get_inv_inertia()));
|
||||
|
||||
|
||||
// Compute limit information
|
||||
real_t hingeAngle = get_hinge_angle();
|
||||
|
||||
// print_line("angle: "+rtos(hingeAngle));
|
||||
//set bias, sign, clear accumulator
|
||||
m_correction = real_t(0.);
|
||||
m_limitSign = real_t(0.);
|
||||
m_solveLimit = false;
|
||||
m_accLimitImpulse = real_t(0.);
|
||||
|
||||
|
||||
|
||||
/*if (m_useLimit) {
|
||||
print_line("low: "+rtos(m_lowerLimit));
|
||||
print_line("hi: "+rtos(m_upperLimit));
|
||||
}*/
|
||||
|
||||
// if (m_lowerLimit < m_upperLimit)
|
||||
if (m_useLimit && m_lowerLimit <= m_upperLimit)
|
||||
{
|
||||
// if (hingeAngle <= m_lowerLimit*m_limitSoftness)
|
||||
if (hingeAngle <= m_lowerLimit)
|
||||
{
|
||||
m_correction = (m_lowerLimit - hingeAngle);
|
||||
m_limitSign = 1.0f;
|
||||
m_solveLimit = true;
|
||||
}
|
||||
// else if (hingeAngle >= m_upperLimit*m_limitSoftness)
|
||||
else if (hingeAngle >= m_upperLimit)
|
||||
{
|
||||
m_correction = m_upperLimit - hingeAngle;
|
||||
m_limitSign = -1.0f;
|
||||
m_solveLimit = true;
|
||||
}
|
||||
}
|
||||
|
||||
//Compute K = J*W*J' for hinge axis
|
||||
Vector3 axisA = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) );
|
||||
m_kHinge = 1.0f / (A->compute_angular_impulse_denominator(axisA) +
|
||||
B->compute_angular_impulse_denominator(axisA));
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void HingeJointSW::solve(float p_step) {
|
||||
|
||||
Vector3 pivotAInW = A->get_transform().xform(m_rbAFrame.origin);
|
||||
Vector3 pivotBInW = B->get_transform().xform(m_rbBFrame.origin);
|
||||
|
||||
//real_t tau = real_t(0.3);
|
||||
|
||||
//linear part
|
||||
if (!m_angularOnly)
|
||||
{
|
||||
Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
|
||||
Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
|
||||
|
||||
Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1);
|
||||
Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2);
|
||||
Vector3 vel = vel1 - vel2;
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
const Vector3& normal = m_jac[i].m_linearJointAxis;
|
||||
real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal();
|
||||
|
||||
real_t rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
//positional error (zeroth order error)
|
||||
real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
|
||||
real_t impulse = depth*tau/p_step * jacDiagABInv - rel_vel * jacDiagABInv;
|
||||
m_appliedImpulse += impulse;
|
||||
Vector3 impulse_vector = normal * impulse;
|
||||
A->apply_impulse(pivotAInW - A->get_transform().origin,impulse_vector);
|
||||
B->apply_impulse(pivotBInW - B->get_transform().origin,-impulse_vector);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
{
|
||||
///solve angular part
|
||||
|
||||
// get axes in world space
|
||||
Vector3 axisA = A->get_transform().basis.xform( m_rbAFrame.basis.get_axis(2) );
|
||||
Vector3 axisB = B->get_transform().basis.xform( m_rbBFrame.basis.get_axis(2) );
|
||||
|
||||
const Vector3& angVelA = A->get_angular_velocity();
|
||||
const Vector3& angVelB = B->get_angular_velocity();
|
||||
|
||||
Vector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
|
||||
Vector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);
|
||||
|
||||
Vector3 angAorthog = angVelA - angVelAroundHingeAxisA;
|
||||
Vector3 angBorthog = angVelB - angVelAroundHingeAxisB;
|
||||
Vector3 velrelOrthog = angAorthog-angBorthog;
|
||||
{
|
||||
//solve orthogonal angular velocity correction
|
||||
real_t relaxation = real_t(1.);
|
||||
real_t len = velrelOrthog.length();
|
||||
if (len > real_t(0.00001))
|
||||
{
|
||||
Vector3 normal = velrelOrthog.normalized();
|
||||
real_t denom = A->compute_angular_impulse_denominator(normal) +
|
||||
B->compute_angular_impulse_denominator(normal);
|
||||
// scale for mass and relaxation
|
||||
velrelOrthog *= (real_t(1.)/denom) * m_relaxationFactor;
|
||||
}
|
||||
|
||||
//solve angular positional correction
|
||||
Vector3 angularError = -axisA.cross(axisB) *(real_t(1.)/p_step);
|
||||
real_t len2 = angularError.length();
|
||||
if (len2>real_t(0.00001))
|
||||
{
|
||||
Vector3 normal2 = angularError.normalized();
|
||||
real_t denom2 = A->compute_angular_impulse_denominator(normal2) +
|
||||
B->compute_angular_impulse_denominator(normal2);
|
||||
angularError *= (real_t(1.)/denom2) * relaxation;
|
||||
}
|
||||
|
||||
A->apply_torque_impulse(-velrelOrthog+angularError);
|
||||
B->apply_torque_impulse(velrelOrthog-angularError);
|
||||
|
||||
// solve limit
|
||||
if (m_solveLimit)
|
||||
{
|
||||
real_t amplitude = ( (angVelB - angVelA).dot( axisA )*m_relaxationFactor + m_correction* (real_t(1.)/p_step)*m_biasFactor ) * m_limitSign;
|
||||
|
||||
real_t impulseMag = amplitude * m_kHinge;
|
||||
|
||||
// Clamp the accumulated impulse
|
||||
real_t temp = m_accLimitImpulse;
|
||||
m_accLimitImpulse = MAX(m_accLimitImpulse + impulseMag, real_t(0) );
|
||||
impulseMag = m_accLimitImpulse - temp;
|
||||
|
||||
|
||||
Vector3 impulse = axisA * impulseMag * m_limitSign;
|
||||
A->apply_torque_impulse(impulse);
|
||||
B->apply_torque_impulse(-impulse);
|
||||
}
|
||||
}
|
||||
|
||||
//apply motor
|
||||
if (m_enableAngularMotor)
|
||||
{
|
||||
//todo: add limits too
|
||||
Vector3 angularLimit(0,0,0);
|
||||
|
||||
Vector3 velrel = angVelAroundHingeAxisA - angVelAroundHingeAxisB;
|
||||
real_t projRelVel = velrel.dot(axisA);
|
||||
|
||||
real_t desiredMotorVel = m_motorTargetVelocity;
|
||||
real_t motor_relvel = desiredMotorVel - projRelVel;
|
||||
|
||||
real_t unclippedMotorImpulse = m_kHinge * motor_relvel;;
|
||||
//todo: should clip against accumulated impulse
|
||||
real_t clippedMotorImpulse = unclippedMotorImpulse > m_maxMotorImpulse ? m_maxMotorImpulse : unclippedMotorImpulse;
|
||||
clippedMotorImpulse = clippedMotorImpulse < -m_maxMotorImpulse ? -m_maxMotorImpulse : clippedMotorImpulse;
|
||||
Vector3 motorImp = clippedMotorImpulse * axisA;
|
||||
|
||||
A->apply_torque_impulse(motorImp+angularLimit);
|
||||
B->apply_torque_impulse(-motorImp-angularLimit);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
/*
|
||||
void HingeJointSW::updateRHS(real_t timeStep)
|
||||
{
|
||||
(void)timeStep;
|
||||
|
||||
}
|
||||
*/
|
||||
|
||||
static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x)
|
||||
{
|
||||
real_t coeff_1 = Math_PI / 4.0f;
|
||||
real_t coeff_2 = 3.0f * coeff_1;
|
||||
real_t abs_y = Math::abs(y);
|
||||
real_t angle;
|
||||
if (x >= 0.0f) {
|
||||
real_t r = (x - abs_y) / (x + abs_y);
|
||||
angle = coeff_1 - coeff_1 * r;
|
||||
} else {
|
||||
real_t r = (x + abs_y) / (abs_y - x);
|
||||
angle = coeff_2 - coeff_1 * r;
|
||||
}
|
||||
return (y < 0.0f) ? -angle : angle;
|
||||
}
|
||||
|
||||
|
||||
real_t HingeJointSW::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) );
|
||||
|
||||
return atan2fast( swingAxis.dot(refAxis0), swingAxis.dot(refAxis1) );
|
||||
}
|
||||
|
||||
|
||||
void HingeJointSW::set_param(PhysicsServer::HingeJointParam p_param, float p_value) {
|
||||
|
||||
switch (p_param) {
|
||||
|
||||
case PhysicsServer::HINGE_JOINT_BIAS: tau=p_value; break;
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_UPPER: m_upperLimit=p_value; break;
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_LOWER: m_lowerLimit=p_value; break;
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_BIAS: m_biasFactor=p_value; break;
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS: m_limitSoftness=p_value; break;
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: m_relaxationFactor=p_value; break;
|
||||
case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: m_motorTargetVelocity=p_value; break;
|
||||
case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: m_maxMotorImpulse=p_value; break;
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
float HingeJointSW::get_param(PhysicsServer::HingeJointParam p_param) const{
|
||||
|
||||
switch (p_param) {
|
||||
|
||||
case PhysicsServer::HINGE_JOINT_BIAS: return tau;
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_UPPER: return m_upperLimit;
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_LOWER: return m_lowerLimit;
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_BIAS: return m_biasFactor;
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_SOFTNESS: return m_limitSoftness;
|
||||
case PhysicsServer::HINGE_JOINT_LIMIT_RELAXATION: return m_relaxationFactor;
|
||||
case PhysicsServer::HINGE_JOINT_MOTOR_TARGET_VELOCITY: return m_motorTargetVelocity;
|
||||
case PhysicsServer::HINGE_JOINT_MOTOR_MAX_IMPULSE: return m_maxMotorImpulse;
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void HingeJointSW::set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value){
|
||||
|
||||
print_line(p_flag+": "+itos(p_value));
|
||||
switch (p_flag) {
|
||||
case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: m_useLimit=p_value; break;
|
||||
case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR: m_enableAngularMotor=p_value; break;
|
||||
}
|
||||
|
||||
}
|
||||
bool HingeJointSW::get_flag(PhysicsServer::HingeJointFlag p_flag) const{
|
||||
|
||||
switch (p_flag) {
|
||||
case PhysicsServer::HINGE_JOINT_FLAG_USE_LIMIT: return m_useLimit;
|
||||
case PhysicsServer::HINGE_JOINT_FLAG_ENABLE_MOTOR:return m_enableAngularMotor;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
89
servers/physics/joints/hinge_joint_sw.h
Normal file
89
servers/physics/joints/hinge_joint_sw.h
Normal file
@ -0,0 +1,89 @@
|
||||
#ifndef HINGE_JOINT_SW_H
|
||||
#define HINGE_JOINT_SW_H
|
||||
|
||||
#include "servers/physics/joints_sw.h"
|
||||
#include "servers/physics/joints/jacobian_entry_sw.h"
|
||||
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
class HingeJointSW : public JointSW {
|
||||
|
||||
union {
|
||||
struct {
|
||||
BodySW *A;
|
||||
BodySW *B;
|
||||
};
|
||||
|
||||
BodySW *_arr[2];
|
||||
};
|
||||
|
||||
JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints
|
||||
JacobianEntrySW m_jacAng[3]; //2 orthogonal angular constraints+ 1 for limit/motor
|
||||
|
||||
Transform m_rbAFrame; // constraint axii. Assumes z is hinge axis.
|
||||
Transform m_rbBFrame;
|
||||
|
||||
real_t m_motorTargetVelocity;
|
||||
real_t m_maxMotorImpulse;
|
||||
|
||||
real_t m_limitSoftness;
|
||||
real_t m_biasFactor;
|
||||
real_t m_relaxationFactor;
|
||||
|
||||
real_t m_lowerLimit;
|
||||
real_t m_upperLimit;
|
||||
|
||||
real_t m_kHinge;
|
||||
|
||||
real_t m_limitSign;
|
||||
real_t m_correction;
|
||||
|
||||
real_t m_accLimitImpulse;
|
||||
|
||||
real_t tau;
|
||||
|
||||
bool m_useLimit;
|
||||
bool m_angularOnly;
|
||||
bool m_enableAngularMotor;
|
||||
bool m_solveLimit;
|
||||
|
||||
real_t m_appliedImpulse;
|
||||
|
||||
|
||||
public:
|
||||
|
||||
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_HINGE; }
|
||||
|
||||
virtual bool setup(float p_step);
|
||||
virtual void solve(float p_step);
|
||||
|
||||
real_t get_hinge_angle();
|
||||
|
||||
void set_param(PhysicsServer::HingeJointParam p_param, float p_value);
|
||||
float get_param(PhysicsServer::HingeJointParam p_param) const;
|
||||
|
||||
void set_flag(PhysicsServer::HingeJointFlag p_flag, bool p_value);
|
||||
bool get_flag(PhysicsServer::HingeJointFlag p_flag) const;
|
||||
|
||||
HingeJointSW(BodySW* rbA,BodySW* rbB, const Transform& frameA, const Transform& frameB);
|
||||
HingeJointSW(BodySW* rbA,BodySW* rbB, const Vector3& pivotInA,const Vector3& pivotInB, const Vector3& axisInA,const Vector3& axisInB);
|
||||
|
||||
};
|
||||
|
||||
#endif // HINGE_JOINT_SW_H
|
||||
2
servers/physics/joints/jacobian_entry_sw.cpp
Normal file
2
servers/physics/joints/jacobian_entry_sw.cpp
Normal file
@ -0,0 +1,2 @@
|
||||
#include "jacobian_entry_sw.h"
|
||||
|
||||
146
servers/physics/joints/jacobian_entry_sw.h
Normal file
146
servers/physics/joints/jacobian_entry_sw.h
Normal file
@ -0,0 +1,146 @@
|
||||
#ifndef JACOBIAN_ENTRY_SW_H
|
||||
#define JACOBIAN_ENTRY_SW_H
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
#include "transform.h"
|
||||
|
||||
class JacobianEntrySW {
|
||||
public:
|
||||
JacobianEntrySW() {};
|
||||
//constraint between two different rigidbodies
|
||||
JacobianEntrySW(
|
||||
const Matrix3& world2A,
|
||||
const Matrix3& world2B,
|
||||
const Vector3& rel_pos1,const Vector3& rel_pos2,
|
||||
const Vector3& jointAxis,
|
||||
const Vector3& inertiaInvA,
|
||||
const real_t massInvA,
|
||||
const Vector3& inertiaInvB,
|
||||
const real_t massInvB)
|
||||
:m_linearJointAxis(jointAxis)
|
||||
{
|
||||
m_aJ = world2A.xform(rel_pos1.cross(m_linearJointAxis));
|
||||
m_bJ = world2B.xform(rel_pos2.cross(-m_linearJointAxis));
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = inertiaInvB * m_bJ;
|
||||
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ) + massInvB + m_1MinvJt.dot(m_bJ);
|
||||
|
||||
ERR_FAIL_COND(m_Adiag <= real_t(0.0));
|
||||
}
|
||||
|
||||
//angular constraint between two different rigidbodies
|
||||
JacobianEntrySW(const Vector3& jointAxis,
|
||||
const Matrix3& world2A,
|
||||
const Matrix3& world2B,
|
||||
const Vector3& inertiaInvA,
|
||||
const Vector3& inertiaInvB)
|
||||
:m_linearJointAxis(Vector3(real_t(0.),real_t(0.),real_t(0.)))
|
||||
{
|
||||
m_aJ= world2A.xform(jointAxis);
|
||||
m_bJ = world2B.xform(-jointAxis);
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = inertiaInvB * m_bJ;
|
||||
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
|
||||
|
||||
ERR_FAIL_COND(m_Adiag <= real_t(0.0));
|
||||
}
|
||||
|
||||
//angular constraint between two different rigidbodies
|
||||
JacobianEntrySW(const Vector3& axisInA,
|
||||
const Vector3& axisInB,
|
||||
const Vector3& inertiaInvA,
|
||||
const Vector3& inertiaInvB)
|
||||
: m_linearJointAxis(Vector3(real_t(0.),real_t(0.),real_t(0.)))
|
||||
, m_aJ(axisInA)
|
||||
, m_bJ(-axisInB)
|
||||
{
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = inertiaInvB * m_bJ;
|
||||
m_Adiag = m_0MinvJt.dot(m_aJ) + m_1MinvJt.dot(m_bJ);
|
||||
|
||||
ERR_FAIL_COND(m_Adiag <= real_t(0.0));
|
||||
}
|
||||
|
||||
//constraint on one rigidbody
|
||||
JacobianEntrySW(
|
||||
const Matrix3& world2A,
|
||||
const Vector3& rel_pos1,const Vector3& rel_pos2,
|
||||
const Vector3& jointAxis,
|
||||
const Vector3& inertiaInvA,
|
||||
const real_t massInvA)
|
||||
:m_linearJointAxis(jointAxis)
|
||||
{
|
||||
m_aJ= world2A.xform(rel_pos1.cross(jointAxis));
|
||||
m_bJ = world2A.xform(rel_pos2.cross(-jointAxis));
|
||||
m_0MinvJt = inertiaInvA * m_aJ;
|
||||
m_1MinvJt = Vector3(real_t(0.),real_t(0.),real_t(0.));
|
||||
m_Adiag = massInvA + m_0MinvJt.dot(m_aJ);
|
||||
|
||||
ERR_FAIL_COND(m_Adiag <= real_t(0.0));
|
||||
}
|
||||
|
||||
real_t getDiagonal() const { return m_Adiag; }
|
||||
|
||||
// for two constraints on the same rigidbody (for example vehicle friction)
|
||||
real_t getNonDiagonal(const JacobianEntrySW& jacB, const real_t massInvA) const
|
||||
{
|
||||
const JacobianEntrySW& 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 JacobianEntrySW& jacB,const real_t massInvA,const real_t massInvB) const
|
||||
{
|
||||
const JacobianEntrySW& 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;
|
||||
Vector3 lin0 = massInvA * lin ;
|
||||
Vector3 lin1 = massInvB * lin;
|
||||
Vector3 sum = ang0+ang1+lin0+lin1;
|
||||
return sum[0]+sum[1]+sum[2];
|
||||
}
|
||||
|
||||
real_t getRelativeVelocity(const Vector3& linvelA,const Vector3& angvelA,const Vector3& linvelB,const Vector3& angvelB)
|
||||
{
|
||||
Vector3 linrel = linvelA - linvelB;
|
||||
Vector3 angvela = angvelA * m_aJ;
|
||||
Vector3 angvelb = angvelB * m_bJ;
|
||||
linrel *= m_linearJointAxis;
|
||||
angvela += angvelb;
|
||||
angvela += linrel;
|
||||
real_t rel_vel2 = angvela[0]+angvela[1]+angvela[2];
|
||||
return rel_vel2 + CMP_EPSILON;
|
||||
}
|
||||
//private:
|
||||
|
||||
Vector3 m_linearJointAxis;
|
||||
Vector3 m_aJ;
|
||||
Vector3 m_bJ;
|
||||
Vector3 m_0MinvJt;
|
||||
Vector3 m_1MinvJt;
|
||||
//Optimization: can be stored in the w/last component of one of the vectors
|
||||
real_t m_Adiag;
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif // JACOBIAN_ENTRY_SW_H
|
||||
127
servers/physics/joints/pin_joint_sw.cpp
Normal file
127
servers/physics/joints/pin_joint_sw.cpp
Normal file
@ -0,0 +1,127 @@
|
||||
#include "pin_joint_sw.h"
|
||||
|
||||
bool PinJointSW::setup(float p_step) {
|
||||
|
||||
m_appliedImpulse = real_t(0.);
|
||||
|
||||
Vector3 normal(0,0,0);
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
normal[i] = 1;
|
||||
memnew_placement(&m_jac[i],JacobianEntrySW(
|
||||
A->get_transform().basis.transposed(),
|
||||
B->get_transform().basis.transposed(),
|
||||
A->get_transform().xform(m_pivotInA) - A->get_transform().origin,
|
||||
B->get_transform().xform(m_pivotInB) - B->get_transform().origin,
|
||||
normal,
|
||||
A->get_inv_inertia(),
|
||||
A->get_inv_mass(),
|
||||
B->get_inv_inertia(),
|
||||
B->get_inv_mass()));
|
||||
normal[i] = 0;
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void PinJointSW::solve(float p_step){
|
||||
|
||||
Vector3 pivotAInW = A->get_transform().xform(m_pivotInA);
|
||||
Vector3 pivotBInW = B->get_transform().xform(m_pivotInB);
|
||||
|
||||
|
||||
Vector3 normal(0,0,0);
|
||||
|
||||
|
||||
// Vector3 angvelA = A->get_transform().origin.getBasis().transpose() * A->getAngularVelocity();
|
||||
// Vector3 angvelB = B->get_transform().origin.getBasis().transpose() * B->getAngularVelocity();
|
||||
|
||||
for (int i=0;i<3;i++)
|
||||
{
|
||||
normal[i] = 1;
|
||||
real_t jacDiagABInv = real_t(1.) / m_jac[i].getDiagonal();
|
||||
|
||||
Vector3 rel_pos1 = pivotAInW - A->get_transform().origin;
|
||||
Vector3 rel_pos2 = pivotBInW - B->get_transform().origin;
|
||||
//this jacobian entry could be re-used for all iterations
|
||||
|
||||
Vector3 vel1 = A->get_velocity_in_local_point(rel_pos1);
|
||||
Vector3 vel2 = B->get_velocity_in_local_point(rel_pos2);
|
||||
Vector3 vel = vel1 - vel2;
|
||||
|
||||
real_t rel_vel;
|
||||
rel_vel = normal.dot(vel);
|
||||
|
||||
/*
|
||||
//velocity error (first order error)
|
||||
real_t rel_vel = m_jac[i].getRelativeVelocity(A->getLinearVelocity(),angvelA,
|
||||
B->getLinearVelocity(),angvelB);
|
||||
*/
|
||||
|
||||
//positional error (zeroth order error)
|
||||
real_t depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
|
||||
|
||||
real_t impulse = depth*m_tau/p_step * jacDiagABInv - m_damping * rel_vel * jacDiagABInv;
|
||||
|
||||
real_t impulseClamp = m_impulseClamp;
|
||||
if (impulseClamp > 0)
|
||||
{
|
||||
if (impulse < -impulseClamp)
|
||||
impulse = -impulseClamp;
|
||||
if (impulse > impulseClamp)
|
||||
impulse = impulseClamp;
|
||||
}
|
||||
|
||||
m_appliedImpulse+=impulse;
|
||||
Vector3 impulse_vector = normal * impulse;
|
||||
A->apply_impulse(pivotAInW - A->get_transform().origin,impulse_vector);
|
||||
B->apply_impulse(pivotBInW - B->get_transform().origin,-impulse_vector);
|
||||
|
||||
normal[i] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void PinJointSW::set_param(PhysicsServer::PinJointParam p_param,float p_value) {
|
||||
|
||||
switch(p_param) {
|
||||
case PhysicsServer::PIN_JOINT_BIAS: m_tau=p_value; break;
|
||||
case PhysicsServer::PIN_JOINT_DAMPING: m_damping=p_value; break;
|
||||
case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: m_impulseClamp=p_value; break;
|
||||
}
|
||||
}
|
||||
|
||||
float PinJointSW::get_param(PhysicsServer::PinJointParam p_param) const{
|
||||
|
||||
switch(p_param) {
|
||||
case PhysicsServer::PIN_JOINT_BIAS: return m_tau;
|
||||
case PhysicsServer::PIN_JOINT_DAMPING: return m_damping;
|
||||
case PhysicsServer::PIN_JOINT_IMPULSE_CLAMP: return m_impulseClamp;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
PinJointSW::PinJointSW(BodySW* p_body_a,const Vector3& p_pos_a,BodySW* p_body_b,const Vector3& p_pos_b) : JointSW(_arr,2) {
|
||||
|
||||
A=p_body_a;
|
||||
B=p_body_b;
|
||||
m_pivotInA=p_pos_a;
|
||||
m_pivotInB=p_pos_b;
|
||||
|
||||
m_tau=0.3;
|
||||
m_damping=1;
|
||||
m_impulseClamp=0;
|
||||
m_appliedImpulse=0;
|
||||
|
||||
A->add_constraint(this,0);
|
||||
B->add_constraint(this,1);
|
||||
|
||||
|
||||
}
|
||||
|
||||
PinJointSW::~PinJointSW() {
|
||||
|
||||
|
||||
|
||||
}
|
||||
67
servers/physics/joints/pin_joint_sw.h
Normal file
67
servers/physics/joints/pin_joint_sw.h
Normal file
@ -0,0 +1,67 @@
|
||||
#ifndef PIN_JOINT_SW_H
|
||||
#define PIN_JOINT_SW_H
|
||||
|
||||
#include "servers/physics/joints_sw.h"
|
||||
#include "servers/physics/joints/jacobian_entry_sw.h"
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
class PinJointSW : public JointSW {
|
||||
|
||||
union {
|
||||
struct {
|
||||
BodySW *A;
|
||||
BodySW *B;
|
||||
};
|
||||
|
||||
BodySW *_arr[2];
|
||||
};
|
||||
|
||||
|
||||
real_t m_tau; //bias
|
||||
real_t m_damping;
|
||||
real_t m_impulseClamp;
|
||||
real_t m_appliedImpulse;
|
||||
|
||||
JacobianEntrySW m_jac[3]; //3 orthogonal linear constraints
|
||||
|
||||
Vector3 m_pivotInA;
|
||||
Vector3 m_pivotInB;
|
||||
|
||||
public:
|
||||
|
||||
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_PIN; }
|
||||
|
||||
virtual bool setup(float p_step);
|
||||
virtual void solve(float p_step);
|
||||
|
||||
void set_param(PhysicsServer::PinJointParam p_param,float p_value);
|
||||
float get_param(PhysicsServer::PinJointParam p_param) const;
|
||||
|
||||
void set_pos_A(const Vector3& p_pos) { m_pivotInA=p_pos; }
|
||||
void set_pos_B(const Vector3& p_pos) { m_pivotInB=p_pos; }
|
||||
|
||||
Vector3 get_pos_A() { return m_pivotInB; }
|
||||
Vector3 get_pos_B() { return m_pivotInA; }
|
||||
|
||||
PinJointSW(BodySW* p_body_a,const Vector3& p_pos_a,BodySW* p_body_b,const Vector3& p_pos_b);
|
||||
~PinJointSW();
|
||||
};
|
||||
|
||||
|
||||
|
||||
#endif // PIN_JOINT_SW_H
|
||||
439
servers/physics/joints/slider_joint_sw.cpp
Normal file
439
servers/physics/joints/slider_joint_sw.cpp
Normal file
@ -0,0 +1,439 @@
|
||||
#include "slider_joint_sw.h"
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
static _FORCE_INLINE_ real_t atan2fast(real_t y, real_t x)
|
||||
{
|
||||
real_t coeff_1 = Math_PI / 4.0f;
|
||||
real_t coeff_2 = 3.0f * coeff_1;
|
||||
real_t abs_y = Math::abs(y);
|
||||
real_t angle;
|
||||
if (x >= 0.0f) {
|
||||
real_t r = (x - abs_y) / (x + abs_y);
|
||||
angle = coeff_1 - coeff_1 * r;
|
||||
} else {
|
||||
real_t r = (x + abs_y) / (abs_y - x);
|
||||
angle = coeff_2 - coeff_1 * r;
|
||||
}
|
||||
return (y < 0.0f) ? -angle : angle;
|
||||
}
|
||||
|
||||
|
||||
void SliderJointSW::initParams()
|
||||
{
|
||||
m_lowerLinLimit = real_t(1.0);
|
||||
m_upperLinLimit = real_t(-1.0);
|
||||
m_lowerAngLimit = real_t(0.);
|
||||
m_upperAngLimit = real_t(0.);
|
||||
m_softnessDirLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
|
||||
m_restitutionDirLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
|
||||
m_dampingDirLin = real_t(0.);
|
||||
m_softnessDirAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
|
||||
m_restitutionDirAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
|
||||
m_dampingDirAng = real_t(0.);
|
||||
m_softnessOrthoLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
|
||||
m_restitutionOrthoLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
|
||||
m_dampingOrthoLin = SLIDER_CONSTRAINT_DEF_DAMPING;
|
||||
m_softnessOrthoAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
|
||||
m_restitutionOrthoAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
|
||||
m_dampingOrthoAng = SLIDER_CONSTRAINT_DEF_DAMPING;
|
||||
m_softnessLimLin = SLIDER_CONSTRAINT_DEF_SOFTNESS;
|
||||
m_restitutionLimLin = SLIDER_CONSTRAINT_DEF_RESTITUTION;
|
||||
m_dampingLimLin = SLIDER_CONSTRAINT_DEF_DAMPING;
|
||||
m_softnessLimAng = SLIDER_CONSTRAINT_DEF_SOFTNESS;
|
||||
m_restitutionLimAng = SLIDER_CONSTRAINT_DEF_RESTITUTION;
|
||||
m_dampingLimAng = SLIDER_CONSTRAINT_DEF_DAMPING;
|
||||
|
||||
m_poweredLinMotor = false;
|
||||
m_targetLinMotorVelocity = real_t(0.);
|
||||
m_maxLinMotorForce = real_t(0.);
|
||||
m_accumulatedLinMotorImpulse = real_t(0.0);
|
||||
|
||||
m_poweredAngMotor = false;
|
||||
m_targetAngMotorVelocity = real_t(0.);
|
||||
m_maxAngMotorForce = real_t(0.);
|
||||
m_accumulatedAngMotorImpulse = real_t(0.0);
|
||||
|
||||
} // SliderJointSW::initParams()
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
SliderJointSW::SliderJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB)
|
||||
: JointSW(_arr,2)
|
||||
, m_frameInA(frameInA)
|
||||
, m_frameInB(frameInB)
|
||||
{
|
||||
|
||||
A=rbA;
|
||||
B=rbB;
|
||||
|
||||
A->add_constraint(this,0);
|
||||
B->add_constraint(this,1);
|
||||
|
||||
initParams();
|
||||
} // SliderJointSW::SliderJointSW()
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
bool SliderJointSW::setup(float p_step)
|
||||
{
|
||||
|
||||
//calculate transforms
|
||||
m_calculatedTransformA = A->get_transform() * m_frameInA;
|
||||
m_calculatedTransformB = B->get_transform() * m_frameInB;
|
||||
m_realPivotAInW = m_calculatedTransformA.origin;
|
||||
m_realPivotBInW = m_calculatedTransformB.origin;
|
||||
m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X
|
||||
m_delta = m_realPivotBInW - m_realPivotAInW;
|
||||
m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
|
||||
m_relPosA = m_projPivotInW - A->get_transform().origin;
|
||||
m_relPosB = m_realPivotBInW - B->get_transform().origin;
|
||||
Vector3 normalWorld;
|
||||
int i;
|
||||
//linear part
|
||||
for(i = 0; i < 3; i++)
|
||||
{
|
||||
normalWorld = m_calculatedTransformA.basis.get_axis(i);
|
||||
memnew_placement(&m_jacLin[i], JacobianEntrySW(
|
||||
A->get_transform().basis.transposed(),
|
||||
B->get_transform().basis.transposed(),
|
||||
m_relPosA,
|
||||
m_relPosB,
|
||||
normalWorld,
|
||||
A->get_inv_inertia(),
|
||||
A->get_inv_mass(),
|
||||
B->get_inv_inertia(),
|
||||
B->get_inv_mass()
|
||||
));
|
||||
m_jacLinDiagABInv[i] = real_t(1.) / m_jacLin[i].getDiagonal();
|
||||
m_depth[i] = m_delta.dot(normalWorld);
|
||||
}
|
||||
testLinLimits();
|
||||
// angular part
|
||||
for(i = 0; i < 3; i++)
|
||||
{
|
||||
normalWorld = m_calculatedTransformA.basis.get_axis(i);
|
||||
memnew_placement(&m_jacAng[i], JacobianEntrySW(
|
||||
normalWorld,
|
||||
A->get_transform().basis.transposed(),
|
||||
B->get_transform().basis.transposed(),
|
||||
A->get_inv_inertia(),
|
||||
B->get_inv_inertia()
|
||||
));
|
||||
}
|
||||
testAngLimits();
|
||||
Vector3 axisA = m_calculatedTransformA.basis.get_axis(0);
|
||||
m_kAngle = real_t(1.0 )/ (A->compute_angular_impulse_denominator(axisA) + B->compute_angular_impulse_denominator(axisA));
|
||||
// clear accumulator for motors
|
||||
m_accumulatedLinMotorImpulse = real_t(0.0);
|
||||
m_accumulatedAngMotorImpulse = real_t(0.0);
|
||||
|
||||
return true;
|
||||
} // SliderJointSW::buildJacobianInt()
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
void SliderJointSW::solve(real_t p_step) {
|
||||
|
||||
int i;
|
||||
// linear
|
||||
Vector3 velA = A->get_velocity_in_local_point(m_relPosA);
|
||||
Vector3 velB = B->get_velocity_in_local_point(m_relPosB);
|
||||
Vector3 vel = velA - velB;
|
||||
for(i = 0; i < 3; i++)
|
||||
{
|
||||
const Vector3& normal = m_jacLin[i].m_linearJointAxis;
|
||||
real_t rel_vel = normal.dot(vel);
|
||||
// calculate positional error
|
||||
real_t depth = m_depth[i];
|
||||
// get parameters
|
||||
real_t softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
|
||||
real_t restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
|
||||
real_t damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
|
||||
// calcutate and apply impulse
|
||||
real_t normalImpulse = softness * (restitution * depth / p_step - damping * rel_vel) * m_jacLinDiagABInv[i];
|
||||
Vector3 impulse_vector = normal * normalImpulse;
|
||||
A->apply_impulse( m_relPosA, impulse_vector);
|
||||
B->apply_impulse(m_relPosB,-impulse_vector);
|
||||
if(m_poweredLinMotor && (!i))
|
||||
{ // apply linear motor
|
||||
if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
|
||||
{
|
||||
real_t desiredMotorVel = m_targetLinMotorVelocity;
|
||||
real_t motor_relvel = desiredMotorVel + rel_vel;
|
||||
normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
|
||||
// clamp accumulated impulse
|
||||
real_t new_acc = m_accumulatedLinMotorImpulse + Math::abs(normalImpulse);
|
||||
if(new_acc > m_maxLinMotorForce)
|
||||
{
|
||||
new_acc = m_maxLinMotorForce;
|
||||
}
|
||||
real_t del = new_acc - m_accumulatedLinMotorImpulse;
|
||||
if(normalImpulse < real_t(0.0))
|
||||
{
|
||||
normalImpulse = -del;
|
||||
}
|
||||
else
|
||||
{
|
||||
normalImpulse = del;
|
||||
}
|
||||
m_accumulatedLinMotorImpulse = new_acc;
|
||||
// apply clamped impulse
|
||||
impulse_vector = normal * normalImpulse;
|
||||
A->apply_impulse( m_relPosA, impulse_vector);
|
||||
B->apply_impulse( m_relPosB,-impulse_vector);
|
||||
}
|
||||
}
|
||||
}
|
||||
// angular
|
||||
// get axes in world space
|
||||
Vector3 axisA = m_calculatedTransformA.basis.get_axis(0);
|
||||
Vector3 axisB = m_calculatedTransformB.basis.get_axis(0);
|
||||
|
||||
const Vector3& angVelA = A->get_angular_velocity();
|
||||
const Vector3& angVelB = B->get_angular_velocity();
|
||||
|
||||
Vector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
|
||||
Vector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);
|
||||
|
||||
Vector3 angAorthog = angVelA - angVelAroundAxisA;
|
||||
Vector3 angBorthog = angVelB - angVelAroundAxisB;
|
||||
Vector3 velrelOrthog = angAorthog-angBorthog;
|
||||
//solve orthogonal angular velocity correction
|
||||
real_t len = velrelOrthog.length();
|
||||
if (len > real_t(0.00001))
|
||||
{
|
||||
Vector3 normal = velrelOrthog.normalized();
|
||||
real_t denom = A->compute_angular_impulse_denominator(normal) + B->compute_angular_impulse_denominator(normal);
|
||||
velrelOrthog *= (real_t(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
|
||||
}
|
||||
//solve angular positional correction
|
||||
Vector3 angularError = axisA.cross(axisB) *(real_t(1.)/p_step);
|
||||
real_t len2 = angularError.length();
|
||||
if (len2>real_t(0.00001))
|
||||
{
|
||||
Vector3 normal2 = angularError.normalized();
|
||||
real_t denom2 = A->compute_angular_impulse_denominator(normal2) + B->compute_angular_impulse_denominator(normal2);
|
||||
angularError *= (real_t(1.)/denom2) * m_restitutionOrthoAng * m_softnessOrthoAng;
|
||||
}
|
||||
// apply impulse
|
||||
A->apply_torque_impulse(-velrelOrthog+angularError);
|
||||
B->apply_torque_impulse(velrelOrthog-angularError);
|
||||
real_t impulseMag;
|
||||
//solve angular limits
|
||||
if(m_solveAngLim)
|
||||
{
|
||||
impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingLimAng + m_angDepth * m_restitutionLimAng / p_step;
|
||||
impulseMag *= m_kAngle * m_softnessLimAng;
|
||||
}
|
||||
else
|
||||
{
|
||||
impulseMag = (angVelB - angVelA).dot(axisA) * m_dampingDirAng + m_angDepth * m_restitutionDirAng / p_step;
|
||||
impulseMag *= m_kAngle * m_softnessDirAng;
|
||||
}
|
||||
Vector3 impulse = axisA * impulseMag;
|
||||
A->apply_torque_impulse(impulse);
|
||||
B->apply_torque_impulse(-impulse);
|
||||
//apply angular motor
|
||||
if(m_poweredAngMotor)
|
||||
{
|
||||
if(m_accumulatedAngMotorImpulse < m_maxAngMotorForce)
|
||||
{
|
||||
Vector3 velrel = angVelAroundAxisA - angVelAroundAxisB;
|
||||
real_t projRelVel = velrel.dot(axisA);
|
||||
|
||||
real_t desiredMotorVel = m_targetAngMotorVelocity;
|
||||
real_t motor_relvel = desiredMotorVel - projRelVel;
|
||||
|
||||
real_t angImpulse = m_kAngle * motor_relvel;
|
||||
// clamp accumulated impulse
|
||||
real_t new_acc = m_accumulatedAngMotorImpulse + Math::abs(angImpulse);
|
||||
if(new_acc > m_maxAngMotorForce)
|
||||
{
|
||||
new_acc = m_maxAngMotorForce;
|
||||
}
|
||||
real_t del = new_acc - m_accumulatedAngMotorImpulse;
|
||||
if(angImpulse < real_t(0.0))
|
||||
{
|
||||
angImpulse = -del;
|
||||
}
|
||||
else
|
||||
{
|
||||
angImpulse = del;
|
||||
}
|
||||
m_accumulatedAngMotorImpulse = new_acc;
|
||||
// apply clamped impulse
|
||||
Vector3 motorImp = angImpulse * axisA;
|
||||
A->apply_torque_impulse(motorImp);
|
||||
B->apply_torque_impulse(-motorImp);
|
||||
}
|
||||
}
|
||||
} // SliderJointSW::solveConstraint()
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
void SliderJointSW::calculateTransforms(void){
|
||||
m_calculatedTransformA = A->get_transform() * m_frameInA ;
|
||||
m_calculatedTransformB = B->get_transform() * m_frameInB;
|
||||
m_realPivotAInW = m_calculatedTransformA.origin;
|
||||
m_realPivotBInW = m_calculatedTransformB.origin;
|
||||
m_sliderAxis = m_calculatedTransformA.basis.get_axis(0); // along X
|
||||
m_delta = m_realPivotBInW - m_realPivotAInW;
|
||||
m_projPivotInW = m_realPivotAInW + m_sliderAxis.dot(m_delta) * m_sliderAxis;
|
||||
Vector3 normalWorld;
|
||||
int i;
|
||||
//linear part
|
||||
for(i = 0; i < 3; i++)
|
||||
{
|
||||
normalWorld = m_calculatedTransformA.basis.get_axis(i);
|
||||
m_depth[i] = m_delta.dot(normalWorld);
|
||||
}
|
||||
} // SliderJointSW::calculateTransforms()
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
void SliderJointSW::testLinLimits(void)
|
||||
{
|
||||
m_solveLinLim = false;
|
||||
m_linPos = m_depth[0];
|
||||
if(m_lowerLinLimit <= m_upperLinLimit)
|
||||
{
|
||||
if(m_depth[0] > m_upperLinLimit)
|
||||
{
|
||||
m_depth[0] -= m_upperLinLimit;
|
||||
m_solveLinLim = true;
|
||||
}
|
||||
else if(m_depth[0] < m_lowerLinLimit)
|
||||
{
|
||||
m_depth[0] -= m_lowerLinLimit;
|
||||
m_solveLinLim = true;
|
||||
}
|
||||
else
|
||||
{
|
||||
m_depth[0] = real_t(0.);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
m_depth[0] = real_t(0.);
|
||||
}
|
||||
} // SliderJointSW::testLinLimits()
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
void SliderJointSW::testAngLimits(void)
|
||||
{
|
||||
m_angDepth = real_t(0.);
|
||||
m_solveAngLim = false;
|
||||
if(m_lowerAngLimit <= m_upperAngLimit)
|
||||
{
|
||||
const Vector3 axisA0 = m_calculatedTransformA.basis.get_axis(1);
|
||||
const Vector3 axisA1 = m_calculatedTransformA.basis.get_axis(2);
|
||||
const Vector3 axisB0 = m_calculatedTransformB.basis.get_axis(1);
|
||||
real_t rot = atan2fast(axisB0.dot(axisA1), axisB0.dot(axisA0));
|
||||
if(rot < m_lowerAngLimit)
|
||||
{
|
||||
m_angDepth = rot - m_lowerAngLimit;
|
||||
m_solveAngLim = true;
|
||||
}
|
||||
else if(rot > m_upperAngLimit)
|
||||
{
|
||||
m_angDepth = rot - m_upperAngLimit;
|
||||
m_solveAngLim = true;
|
||||
}
|
||||
}
|
||||
} // SliderJointSW::testAngLimits()
|
||||
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
|
||||
|
||||
Vector3 SliderJointSW::getAncorInA(void)
|
||||
{
|
||||
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 SliderJointSW::getAncorInB(void)
|
||||
{
|
||||
Vector3 ancorInB;
|
||||
ancorInB = m_frameInB.origin;
|
||||
return ancorInB;
|
||||
} // SliderJointSW::getAncorInB();
|
||||
|
||||
void SliderJointSW::set_param(PhysicsServer::SliderJointParam p_param, float p_value) {
|
||||
|
||||
switch(p_param) {
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: m_upperLinLimit=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: m_lowerLinLimit=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: m_softnessLimLin=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: m_restitutionLimLin=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: m_dampingLimLin=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: m_softnessDirLin=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: m_restitutionDirLin=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: m_dampingDirLin=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoLin=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoLin=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: m_dampingOrthoLin=p_value; break;
|
||||
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: m_upperAngLimit=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: m_lowerAngLimit=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: m_softnessLimAng=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: m_restitutionLimAng=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: m_dampingLimAng=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: m_softnessDirAng=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: m_restitutionDirAng=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: m_dampingDirAng=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: m_softnessOrthoAng=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: m_restitutionOrthoAng=p_value; break;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: m_dampingOrthoAng=p_value; break;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
float SliderJointSW::get_param(PhysicsServer::SliderJointParam p_param) const {
|
||||
|
||||
switch(p_param) {
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_UPPER: return m_upperLinLimit;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_LOWER: return m_lowerLinLimit;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS: return m_softnessLimLin;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION: return m_restitutionLimLin;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_LIMIT_DAMPING: return m_dampingLimLin;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_SOFTNESS: return m_softnessDirLin;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_RESTITUTION: return m_restitutionDirLin;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_MOTION_DAMPING: return m_dampingDirLin;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS: return m_softnessOrthoLin;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION: return m_restitutionOrthoLin;
|
||||
case PhysicsServer::SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING: return m_dampingOrthoLin;
|
||||
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_UPPER: return m_upperAngLimit;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_LOWER: return m_lowerAngLimit;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS: return m_softnessLimAng;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION: return m_restitutionLimAng;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_LIMIT_DAMPING: return m_dampingLimAng;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS: return m_softnessDirAng;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION: return m_restitutionDirAng;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_MOTION_DAMPING: return m_dampingDirAng;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS: return m_softnessOrthoAng;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION: return m_restitutionOrthoAng;
|
||||
case PhysicsServer::SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING: return m_dampingOrthoAng;
|
||||
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
||||
}
|
||||
|
||||
|
||||
218
servers/physics/joints/slider_joint_sw.h
Normal file
218
servers/physics/joints/slider_joint_sw.h
Normal file
@ -0,0 +1,218 @@
|
||||
#ifndef SLIDER_JOINT_SW_H
|
||||
#define SLIDER_JOINT_SW_H
|
||||
|
||||
#include "servers/physics/joints_sw.h"
|
||||
#include "servers/physics/joints/jacobian_entry_sw.h"
|
||||
|
||||
|
||||
/*
|
||||
Bullet Continuous Collision Detection and Physics Library
|
||||
Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/
|
||||
|
||||
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.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
/*
|
||||
Added by Roman Ponomarev (rponom@gmail.com)
|
||||
April 04, 2008
|
||||
|
||||
*/
|
||||
|
||||
#define SLIDER_CONSTRAINT_DEF_SOFTNESS (real_t(1.0))
|
||||
#define SLIDER_CONSTRAINT_DEF_DAMPING (real_t(1.0))
|
||||
#define SLIDER_CONSTRAINT_DEF_RESTITUTION (real_t(0.7))
|
||||
|
||||
//-----------------------------------------------------------------------------
|
||||
|
||||
class SliderJointSW : public JointSW {
|
||||
protected:
|
||||
|
||||
union {
|
||||
struct {
|
||||
BodySW *A;
|
||||
BodySW *B;
|
||||
};
|
||||
|
||||
BodySW *_arr[2];
|
||||
};
|
||||
|
||||
Transform m_frameInA;
|
||||
Transform m_frameInB;
|
||||
|
||||
// linear limits
|
||||
real_t m_lowerLinLimit;
|
||||
real_t m_upperLinLimit;
|
||||
// angular limits
|
||||
real_t m_lowerAngLimit;
|
||||
real_t m_upperAngLimit;
|
||||
// softness, restitution and damping for different cases
|
||||
// DirLin - moving inside linear limits
|
||||
// LimLin - hitting linear limit
|
||||
// DirAng - moving inside angular limits
|
||||
// LimAng - hitting angular limit
|
||||
// OrthoLin, OrthoAng - against constraint axis
|
||||
real_t m_softnessDirLin;
|
||||
real_t m_restitutionDirLin;
|
||||
real_t m_dampingDirLin;
|
||||
real_t m_softnessDirAng;
|
||||
real_t m_restitutionDirAng;
|
||||
real_t m_dampingDirAng;
|
||||
real_t m_softnessLimLin;
|
||||
real_t m_restitutionLimLin;
|
||||
real_t m_dampingLimLin;
|
||||
real_t m_softnessLimAng;
|
||||
real_t m_restitutionLimAng;
|
||||
real_t m_dampingLimAng;
|
||||
real_t m_softnessOrthoLin;
|
||||
real_t m_restitutionOrthoLin;
|
||||
real_t m_dampingOrthoLin;
|
||||
real_t m_softnessOrthoAng;
|
||||
real_t m_restitutionOrthoAng;
|
||||
real_t m_dampingOrthoAng;
|
||||
|
||||
// for interlal use
|
||||
bool m_solveLinLim;
|
||||
bool m_solveAngLim;
|
||||
|
||||
JacobianEntrySW m_jacLin[3];
|
||||
real_t m_jacLinDiagABInv[3];
|
||||
|
||||
JacobianEntrySW m_jacAng[3];
|
||||
|
||||
real_t m_timeStep;
|
||||
Transform m_calculatedTransformA;
|
||||
Transform m_calculatedTransformB;
|
||||
|
||||
Vector3 m_sliderAxis;
|
||||
Vector3 m_realPivotAInW;
|
||||
Vector3 m_realPivotBInW;
|
||||
Vector3 m_projPivotInW;
|
||||
Vector3 m_delta;
|
||||
Vector3 m_depth;
|
||||
Vector3 m_relPosA;
|
||||
Vector3 m_relPosB;
|
||||
|
||||
real_t m_linPos;
|
||||
|
||||
real_t m_angDepth;
|
||||
real_t m_kAngle;
|
||||
|
||||
bool m_poweredLinMotor;
|
||||
real_t m_targetLinMotorVelocity;
|
||||
real_t m_maxLinMotorForce;
|
||||
real_t m_accumulatedLinMotorImpulse;
|
||||
|
||||
bool m_poweredAngMotor;
|
||||
real_t m_targetAngMotorVelocity;
|
||||
real_t m_maxAngMotorForce;
|
||||
real_t m_accumulatedAngMotorImpulse;
|
||||
|
||||
//------------------------
|
||||
void initParams();
|
||||
public:
|
||||
// constructors
|
||||
SliderJointSW(BodySW* rbA, BodySW* rbB, const Transform& frameInA, const Transform& frameInB);
|
||||
//SliderJointSW();
|
||||
// overrides
|
||||
|
||||
// access
|
||||
const BodySW* getRigidBodyA() const { return A; }
|
||||
const BodySW* getRigidBodyB() const { return B; }
|
||||
const Transform & getCalculatedTransformA() const { return m_calculatedTransformA; }
|
||||
const Transform & getCalculatedTransformB() const { return m_calculatedTransformB; }
|
||||
const Transform & getFrameOffsetA() const { return m_frameInA; }
|
||||
const Transform & getFrameOffsetB() const { return m_frameInB; }
|
||||
Transform & getFrameOffsetA() { return m_frameInA; }
|
||||
Transform & getFrameOffsetB() { return m_frameInB; }
|
||||
real_t getLowerLinLimit() { return m_lowerLinLimit; }
|
||||
void setLowerLinLimit(real_t lowerLimit) { m_lowerLinLimit = lowerLimit; }
|
||||
real_t getUpperLinLimit() { return m_upperLinLimit; }
|
||||
void setUpperLinLimit(real_t upperLimit) { m_upperLinLimit = upperLimit; }
|
||||
real_t getLowerAngLimit() { return m_lowerAngLimit; }
|
||||
void setLowerAngLimit(real_t lowerLimit) { m_lowerAngLimit = lowerLimit; }
|
||||
real_t getUpperAngLimit() { return m_upperAngLimit; }
|
||||
void setUpperAngLimit(real_t upperLimit) { m_upperAngLimit = upperLimit; }
|
||||
|
||||
real_t getSoftnessDirLin() { return m_softnessDirLin; }
|
||||
real_t getRestitutionDirLin() { return m_restitutionDirLin; }
|
||||
real_t getDampingDirLin() { return m_dampingDirLin ; }
|
||||
real_t getSoftnessDirAng() { return m_softnessDirAng; }
|
||||
real_t getRestitutionDirAng() { return m_restitutionDirAng; }
|
||||
real_t getDampingDirAng() { return m_dampingDirAng; }
|
||||
real_t getSoftnessLimLin() { return m_softnessLimLin; }
|
||||
real_t getRestitutionLimLin() { return m_restitutionLimLin; }
|
||||
real_t getDampingLimLin() { return m_dampingLimLin; }
|
||||
real_t getSoftnessLimAng() { return m_softnessLimAng; }
|
||||
real_t getRestitutionLimAng() { return m_restitutionLimAng; }
|
||||
real_t getDampingLimAng() { return m_dampingLimAng; }
|
||||
real_t getSoftnessOrthoLin() { return m_softnessOrthoLin; }
|
||||
real_t getRestitutionOrthoLin() { return m_restitutionOrthoLin; }
|
||||
real_t getDampingOrthoLin() { return m_dampingOrthoLin; }
|
||||
real_t getSoftnessOrthoAng() { return m_softnessOrthoAng; }
|
||||
real_t getRestitutionOrthoAng() { return m_restitutionOrthoAng; }
|
||||
real_t getDampingOrthoAng() { return m_dampingOrthoAng; }
|
||||
void setSoftnessDirLin(real_t softnessDirLin) { m_softnessDirLin = softnessDirLin; }
|
||||
void setRestitutionDirLin(real_t restitutionDirLin) { m_restitutionDirLin = restitutionDirLin; }
|
||||
void setDampingDirLin(real_t dampingDirLin) { m_dampingDirLin = dampingDirLin; }
|
||||
void setSoftnessDirAng(real_t softnessDirAng) { m_softnessDirAng = softnessDirAng; }
|
||||
void setRestitutionDirAng(real_t restitutionDirAng) { m_restitutionDirAng = restitutionDirAng; }
|
||||
void setDampingDirAng(real_t dampingDirAng) { m_dampingDirAng = dampingDirAng; }
|
||||
void setSoftnessLimLin(real_t softnessLimLin) { m_softnessLimLin = softnessLimLin; }
|
||||
void setRestitutionLimLin(real_t restitutionLimLin) { m_restitutionLimLin = restitutionLimLin; }
|
||||
void setDampingLimLin(real_t dampingLimLin) { m_dampingLimLin = dampingLimLin; }
|
||||
void setSoftnessLimAng(real_t softnessLimAng) { m_softnessLimAng = softnessLimAng; }
|
||||
void setRestitutionLimAng(real_t restitutionLimAng) { m_restitutionLimAng = restitutionLimAng; }
|
||||
void setDampingLimAng(real_t dampingLimAng) { m_dampingLimAng = dampingLimAng; }
|
||||
void setSoftnessOrthoLin(real_t softnessOrthoLin) { m_softnessOrthoLin = softnessOrthoLin; }
|
||||
void setRestitutionOrthoLin(real_t restitutionOrthoLin) { m_restitutionOrthoLin = restitutionOrthoLin; }
|
||||
void setDampingOrthoLin(real_t dampingOrthoLin) { m_dampingOrthoLin = dampingOrthoLin; }
|
||||
void setSoftnessOrthoAng(real_t softnessOrthoAng) { m_softnessOrthoAng = softnessOrthoAng; }
|
||||
void setRestitutionOrthoAng(real_t restitutionOrthoAng) { m_restitutionOrthoAng = restitutionOrthoAng; }
|
||||
void setDampingOrthoAng(real_t dampingOrthoAng) { m_dampingOrthoAng = dampingOrthoAng; }
|
||||
void setPoweredLinMotor(bool onOff) { m_poweredLinMotor = onOff; }
|
||||
bool getPoweredLinMotor() { return m_poweredLinMotor; }
|
||||
void setTargetLinMotorVelocity(real_t targetLinMotorVelocity) { m_targetLinMotorVelocity = targetLinMotorVelocity; }
|
||||
real_t getTargetLinMotorVelocity() { return m_targetLinMotorVelocity; }
|
||||
void setMaxLinMotorForce(real_t maxLinMotorForce) { m_maxLinMotorForce = maxLinMotorForce; }
|
||||
real_t getMaxLinMotorForce() { return m_maxLinMotorForce; }
|
||||
void setPoweredAngMotor(bool onOff) { m_poweredAngMotor = onOff; }
|
||||
bool getPoweredAngMotor() { return m_poweredAngMotor; }
|
||||
void setTargetAngMotorVelocity(real_t targetAngMotorVelocity) { m_targetAngMotorVelocity = targetAngMotorVelocity; }
|
||||
real_t getTargetAngMotorVelocity() { return m_targetAngMotorVelocity; }
|
||||
void setMaxAngMotorForce(real_t maxAngMotorForce) { m_maxAngMotorForce = maxAngMotorForce; }
|
||||
real_t getMaxAngMotorForce() { return m_maxAngMotorForce; }
|
||||
real_t getLinearPos() { return m_linPos; }
|
||||
|
||||
// access for ODE solver
|
||||
bool getSolveLinLimit() { return m_solveLinLim; }
|
||||
real_t getLinDepth() { return m_depth[0]; }
|
||||
bool getSolveAngLimit() { return m_solveAngLim; }
|
||||
real_t getAngDepth() { return m_angDepth; }
|
||||
// shared code used by ODE solver
|
||||
void calculateTransforms(void);
|
||||
void testLinLimits(void);
|
||||
void testAngLimits(void);
|
||||
// access for PE Solver
|
||||
Vector3 getAncorInA(void);
|
||||
Vector3 getAncorInB(void);
|
||||
|
||||
void set_param(PhysicsServer::SliderJointParam p_param, float p_value);
|
||||
float get_param(PhysicsServer::SliderJointParam p_param) const;
|
||||
|
||||
bool setup(float p_step);
|
||||
void solve(float p_step);
|
||||
|
||||
virtual PhysicsServer::JointType get_type() const { return PhysicsServer::JOINT_SLIDER; }
|
||||
|
||||
};
|
||||
|
||||
|
||||
#endif // SLIDER_JOINT_SW_H
|
||||
@ -36,22 +36,12 @@
|
||||
|
||||
class JointSW : public ConstraintSW {
|
||||
|
||||
real_t max_force;
|
||||
real_t bias;
|
||||
real_t max_bias;
|
||||
|
||||
public:
|
||||
|
||||
_FORCE_INLINE_ void set_max_force(real_t p_force) { max_force=p_force; }
|
||||
_FORCE_INLINE_ real_t get_max_force() const { return max_force; }
|
||||
|
||||
_FORCE_INLINE_ void set_bias(real_t p_bias) { bias=p_bias; }
|
||||
_FORCE_INLINE_ real_t get_bias() const { return bias; }
|
||||
|
||||
_FORCE_INLINE_ void set_max_bias(real_t p_bias) { max_bias=p_bias; }
|
||||
_FORCE_INLINE_ real_t get_max_bias() const { return max_bias; }
|
||||
|
||||
// virtual PhysicsServer::JointType get_type() const=0;
|
||||
JointSW(BodySW **p_body_ptr=NULL,int p_body_count=0) : ConstraintSW(p_body_ptr,p_body_count) { bias=0; max_force=max_bias=3.40282e+38; };
|
||||
virtual PhysicsServer::JointType get_type() const=0;
|
||||
_FORCE_INLINE_ JointSW(BodySW **p_body_ptr=NULL,int p_body_count=0) : ConstraintSW(p_body_ptr,p_body_count) {
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
@ -29,6 +29,12 @@
|
||||
#include "physics_server_sw.h"
|
||||
#include "broad_phase_basic.h"
|
||||
#include "broad_phase_octree.h"
|
||||
#include "joints/pin_joint_sw.h"
|
||||
#include "joints/hinge_joint_sw.h"
|
||||
#include "joints/slider_joint_sw.h"
|
||||
#include "joints/cone_twist_joint_sw.h"
|
||||
#include "joints/generic_6dof_joint_sw.h"
|
||||
|
||||
|
||||
RID PhysicsServerSW::shape_create(ShapeType p_shape) {
|
||||
|
||||
@ -137,6 +143,10 @@ RID PhysicsServerSW::space_create() {
|
||||
space->set_default_area(area);
|
||||
area->set_space(space);
|
||||
area->set_priority(-1);
|
||||
RID sgb = body_create();
|
||||
body_set_space(sgb,id);
|
||||
body_set_mode(sgb,BODY_MODE_STATIC);
|
||||
space->set_static_global_body(sgb);
|
||||
|
||||
return id;
|
||||
};
|
||||
@ -394,6 +404,25 @@ void PhysicsServerSW::area_set_monitor_callback(RID p_area,Object *p_receiver,co
|
||||
|
||||
}
|
||||
|
||||
void PhysicsServerSW::area_set_ray_pickable(RID p_area,bool p_enable) {
|
||||
|
||||
AreaSW *area = area_owner.get(p_area);
|
||||
ERR_FAIL_COND(!area);
|
||||
|
||||
area->set_ray_pickable(p_enable);
|
||||
|
||||
}
|
||||
|
||||
bool PhysicsServerSW::area_is_ray_pickable(RID p_area) const{
|
||||
|
||||
AreaSW *area = area_owner.get(p_area);
|
||||
ERR_FAIL_COND_V(!area,false);
|
||||
|
||||
return area->is_ray_pickable();
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
/* BODY API */
|
||||
|
||||
@ -810,6 +839,308 @@ void PhysicsServerSW::body_set_force_integration_callback(RID p_body,Object *p_r
|
||||
|
||||
/* JOINT API */
|
||||
|
||||
RID PhysicsServerSW::joint_create_pin(RID p_body_A,const Vector3& p_local_A,RID p_body_B,const Vector3& p_local_B) {
|
||||
|
||||
BodySW *body_A = body_owner.get(p_body_A);
|
||||
ERR_FAIL_COND_V(!body_A,RID());
|
||||
|
||||
if (!p_body_B.is_valid()) {
|
||||
ERR_FAIL_COND_V(!body_A->get_space(),RID());
|
||||
p_body_B=body_A->get_space()->get_static_global_body();
|
||||
}
|
||||
|
||||
BodySW *body_B = body_owner.get(p_body_B);
|
||||
ERR_FAIL_COND_V(!body_B,RID());
|
||||
|
||||
ERR_FAIL_COND_V(body_A==body_B,RID());
|
||||
|
||||
JointSW *joint = memnew( PinJointSW(body_A,p_local_A,body_B,p_local_B) );
|
||||
RID rid = joint_owner.make_rid(joint);
|
||||
joint->set_self(rid);
|
||||
return rid;
|
||||
}
|
||||
|
||||
void PhysicsServerSW::pin_joint_set_param(RID p_joint,PinJointParam p_param, float p_value){
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND(!joint);
|
||||
ERR_FAIL_COND(joint->get_type()!=JOINT_PIN);
|
||||
PinJointSW *pin_joint = static_cast<PinJointSW*>(joint);
|
||||
pin_joint->set_param(p_param,p_value);
|
||||
|
||||
}
|
||||
float PhysicsServerSW::pin_joint_get_param(RID p_joint,PinJointParam p_param) const{
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND_V(!joint,0);
|
||||
ERR_FAIL_COND_V(joint->get_type()!=JOINT_PIN,0);
|
||||
PinJointSW *pin_joint = static_cast<PinJointSW*>(joint);
|
||||
return pin_joint->get_param(p_param);
|
||||
|
||||
}
|
||||
|
||||
void PhysicsServerSW::pin_joint_set_local_A(RID p_joint, const Vector3& p_A){
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND(!joint);
|
||||
ERR_FAIL_COND(joint->get_type()!=JOINT_PIN);
|
||||
PinJointSW *pin_joint = static_cast<PinJointSW*>(joint);
|
||||
pin_joint->set_pos_A(p_A);
|
||||
|
||||
}
|
||||
Vector3 PhysicsServerSW::pin_joint_get_local_A(RID p_joint) const{
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND_V(!joint,Vector3());
|
||||
ERR_FAIL_COND_V(joint->get_type()!=JOINT_PIN,Vector3());
|
||||
PinJointSW *pin_joint = static_cast<PinJointSW*>(joint);
|
||||
return pin_joint->get_pos_A();
|
||||
|
||||
}
|
||||
|
||||
void PhysicsServerSW::pin_joint_set_local_B(RID p_joint, const Vector3& p_B){
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND(!joint);
|
||||
ERR_FAIL_COND(joint->get_type()!=JOINT_PIN);
|
||||
PinJointSW *pin_joint = static_cast<PinJointSW*>(joint);
|
||||
pin_joint->set_pos_B(p_B);
|
||||
|
||||
}
|
||||
Vector3 PhysicsServerSW::pin_joint_get_local_B(RID p_joint) const{
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND_V(!joint,Vector3());
|
||||
ERR_FAIL_COND_V(joint->get_type()!=JOINT_PIN,Vector3());
|
||||
PinJointSW *pin_joint = static_cast<PinJointSW*>(joint);
|
||||
return pin_joint->get_pos_B();
|
||||
|
||||
}
|
||||
|
||||
|
||||
RID PhysicsServerSW::joint_create_hinge(RID p_body_A,const Transform& p_frame_A,RID p_body_B,const Transform& p_frame_B) {
|
||||
|
||||
BodySW *body_A = body_owner.get(p_body_A);
|
||||
ERR_FAIL_COND_V(!body_A,RID());
|
||||
|
||||
if (!p_body_B.is_valid()) {
|
||||
ERR_FAIL_COND_V(!body_A->get_space(),RID());
|
||||
p_body_B=body_A->get_space()->get_static_global_body();
|
||||
}
|
||||
|
||||
BodySW *body_B = body_owner.get(p_body_B);
|
||||
ERR_FAIL_COND_V(!body_B,RID());
|
||||
|
||||
ERR_FAIL_COND_V(body_A==body_B,RID());
|
||||
|
||||
JointSW *joint = memnew( HingeJointSW(body_A,body_B,p_frame_A,p_frame_B) );
|
||||
RID rid = joint_owner.make_rid(joint);
|
||||
joint->set_self(rid);
|
||||
return rid;
|
||||
|
||||
}
|
||||
|
||||
|
||||
RID PhysicsServerSW::joint_create_hinge_simple(RID p_body_A,const Vector3& p_pivot_A,const Vector3& p_axis_A,RID p_body_B,const Vector3& p_pivot_B,const Vector3& p_axis_B) {
|
||||
|
||||
BodySW *body_A = body_owner.get(p_body_A);
|
||||
ERR_FAIL_COND_V(!body_A,RID());
|
||||
|
||||
if (!p_body_B.is_valid()) {
|
||||
ERR_FAIL_COND_V(!body_A->get_space(),RID());
|
||||
p_body_B=body_A->get_space()->get_static_global_body();
|
||||
}
|
||||
|
||||
BodySW *body_B = body_owner.get(p_body_B);
|
||||
ERR_FAIL_COND_V(!body_B,RID());
|
||||
|
||||
ERR_FAIL_COND_V(body_A==body_B,RID());
|
||||
|
||||
JointSW *joint = memnew( HingeJointSW(body_A,body_B,p_pivot_A,p_pivot_B,p_axis_A,p_axis_B) );
|
||||
RID rid = joint_owner.make_rid(joint);
|
||||
joint->set_self(rid);
|
||||
return rid;
|
||||
|
||||
}
|
||||
|
||||
void PhysicsServerSW::hinge_joint_set_param(RID p_joint,HingeJointParam p_param, float p_value){
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND(!joint);
|
||||
ERR_FAIL_COND(joint->get_type()!=JOINT_HINGE);
|
||||
HingeJointSW *hinge_joint = static_cast<HingeJointSW*>(joint);
|
||||
hinge_joint->set_param(p_param,p_value);
|
||||
|
||||
}
|
||||
float PhysicsServerSW::hinge_joint_get_param(RID p_joint,HingeJointParam p_param) const{
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND_V(!joint,0);
|
||||
ERR_FAIL_COND_V(joint->get_type()!=JOINT_HINGE,0);
|
||||
HingeJointSW *hinge_joint = static_cast<HingeJointSW*>(joint);
|
||||
return hinge_joint->get_param(p_param);
|
||||
|
||||
}
|
||||
|
||||
void PhysicsServerSW::hinge_joint_set_flag(RID p_joint,HingeJointFlag p_flag, bool p_value){
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND(!joint);
|
||||
ERR_FAIL_COND(joint->get_type()!=JOINT_HINGE);
|
||||
HingeJointSW *hinge_joint = static_cast<HingeJointSW*>(joint);
|
||||
hinge_joint->set_flag(p_flag,p_value);
|
||||
|
||||
}
|
||||
bool PhysicsServerSW::hinge_joint_get_flag(RID p_joint,HingeJointFlag p_flag) const{
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND_V(!joint,false);
|
||||
ERR_FAIL_COND_V(joint->get_type()!=JOINT_HINGE,false);
|
||||
HingeJointSW *hinge_joint = static_cast<HingeJointSW*>(joint);
|
||||
return hinge_joint->get_flag(p_flag);
|
||||
}
|
||||
|
||||
PhysicsServerSW::JointType PhysicsServerSW::joint_get_type(RID p_joint) const {
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND_V(!joint,JOINT_PIN);
|
||||
return joint->get_type();
|
||||
}
|
||||
|
||||
RID PhysicsServerSW::joint_create_slider(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B) {
|
||||
|
||||
BodySW *body_A = body_owner.get(p_body_A);
|
||||
ERR_FAIL_COND_V(!body_A,RID());
|
||||
|
||||
if (!p_body_B.is_valid()) {
|
||||
ERR_FAIL_COND_V(!body_A->get_space(),RID());
|
||||
p_body_B=body_A->get_space()->get_static_global_body();
|
||||
}
|
||||
|
||||
BodySW *body_B = body_owner.get(p_body_B);
|
||||
ERR_FAIL_COND_V(!body_B,RID());
|
||||
|
||||
ERR_FAIL_COND_V(body_A==body_B,RID());
|
||||
|
||||
JointSW *joint = memnew( SliderJointSW(body_A,body_B,p_local_frame_A,p_local_frame_B) );
|
||||
RID rid = joint_owner.make_rid(joint);
|
||||
joint->set_self(rid);
|
||||
return rid;
|
||||
}
|
||||
|
||||
void PhysicsServerSW::slider_joint_set_param(RID p_joint,SliderJointParam p_param, float p_value){
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND(!joint);
|
||||
ERR_FAIL_COND(joint->get_type()!=JOINT_SLIDER);
|
||||
SliderJointSW *slider_joint = static_cast<SliderJointSW*>(joint);
|
||||
slider_joint->set_param(p_param,p_value);
|
||||
}
|
||||
float PhysicsServerSW::slider_joint_get_param(RID p_joint,SliderJointParam p_param) const{
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND_V(!joint,0);
|
||||
ERR_FAIL_COND_V(joint->get_type()!=JOINT_CONE_TWIST,0);
|
||||
SliderJointSW *slider_joint = static_cast<SliderJointSW*>(joint);
|
||||
return slider_joint->get_param(p_param);
|
||||
}
|
||||
|
||||
|
||||
RID PhysicsServerSW::joint_create_cone_twist(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B) {
|
||||
|
||||
BodySW *body_A = body_owner.get(p_body_A);
|
||||
ERR_FAIL_COND_V(!body_A,RID());
|
||||
|
||||
if (!p_body_B.is_valid()) {
|
||||
ERR_FAIL_COND_V(!body_A->get_space(),RID());
|
||||
p_body_B=body_A->get_space()->get_static_global_body();
|
||||
}
|
||||
|
||||
BodySW *body_B = body_owner.get(p_body_B);
|
||||
ERR_FAIL_COND_V(!body_B,RID());
|
||||
|
||||
ERR_FAIL_COND_V(body_A==body_B,RID());
|
||||
|
||||
JointSW *joint = memnew( ConeTwistJointSW(body_A,body_B,p_local_frame_A,p_local_frame_B) );
|
||||
RID rid = joint_owner.make_rid(joint);
|
||||
joint->set_self(rid);
|
||||
return rid;
|
||||
}
|
||||
|
||||
void PhysicsServerSW::cone_twist_joint_set_param(RID p_joint,ConeTwistJointParam p_param, float p_value) {
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND(!joint);
|
||||
ERR_FAIL_COND(joint->get_type()!=JOINT_CONE_TWIST);
|
||||
ConeTwistJointSW *cone_twist_joint = static_cast<ConeTwistJointSW*>(joint);
|
||||
cone_twist_joint->set_param(p_param,p_value);
|
||||
}
|
||||
float PhysicsServerSW::cone_twist_joint_get_param(RID p_joint,ConeTwistJointParam p_param) const {
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND_V(!joint,0);
|
||||
ERR_FAIL_COND_V(joint->get_type()!=JOINT_CONE_TWIST,0);
|
||||
ConeTwistJointSW *cone_twist_joint = static_cast<ConeTwistJointSW*>(joint);
|
||||
return cone_twist_joint->get_param(p_param);
|
||||
}
|
||||
|
||||
|
||||
RID PhysicsServerSW::joint_create_generic_6dof(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B) {
|
||||
|
||||
BodySW *body_A = body_owner.get(p_body_A);
|
||||
ERR_FAIL_COND_V(!body_A,RID());
|
||||
|
||||
if (!p_body_B.is_valid()) {
|
||||
ERR_FAIL_COND_V(!body_A->get_space(),RID());
|
||||
p_body_B=body_A->get_space()->get_static_global_body();
|
||||
}
|
||||
|
||||
BodySW *body_B = body_owner.get(p_body_B);
|
||||
ERR_FAIL_COND_V(!body_B,RID());
|
||||
|
||||
ERR_FAIL_COND_V(body_A==body_B,RID());
|
||||
|
||||
JointSW *joint = memnew( Generic6DOFJointSW(body_A,body_B,p_local_frame_A,p_local_frame_B,true) );
|
||||
RID rid = joint_owner.make_rid(joint);
|
||||
joint->set_self(rid);
|
||||
return rid;
|
||||
}
|
||||
|
||||
void PhysicsServerSW::generic_6dof_joint_set_param(RID p_joint,Vector3::Axis p_axis,G6DOFJointAxisParam p_param, float p_value){
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND(!joint);
|
||||
ERR_FAIL_COND(joint->get_type()!=JOINT_6DOF);
|
||||
Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW*>(joint);
|
||||
generic_6dof_joint->set_param(p_axis,p_param,p_value);
|
||||
}
|
||||
float PhysicsServerSW::generic_6dof_joint_get_param(RID p_joint,Vector3::Axis p_axis,G6DOFJointAxisParam p_param){
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND_V(!joint,0);
|
||||
ERR_FAIL_COND_V(joint->get_type()!=JOINT_6DOF,0);
|
||||
Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW*>(joint);
|
||||
return generic_6dof_joint->get_param(p_axis,p_param);
|
||||
}
|
||||
|
||||
void PhysicsServerSW::generic_6dof_joint_set_flag(RID p_joint,Vector3::Axis p_axis,G6DOFJointAxisFlag p_flag, bool p_enable){
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND(!joint);
|
||||
ERR_FAIL_COND(joint->get_type()!=JOINT_6DOF);
|
||||
Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW*>(joint);
|
||||
generic_6dof_joint->set_flag(p_axis,p_flag,p_enable);
|
||||
}
|
||||
bool PhysicsServerSW::generic_6dof_joint_get_flag(RID p_joint,Vector3::Axis p_axis,G6DOFJointAxisFlag p_flag){
|
||||
|
||||
JointSW *joint = joint_owner.get(p_joint);
|
||||
ERR_FAIL_COND_V(!joint,false);
|
||||
ERR_FAIL_COND_V(joint->get_type()!=JOINT_6DOF,false);
|
||||
Generic6DOFJointSW *generic_6dof_joint = static_cast<Generic6DOFJointSW*>(joint);
|
||||
return generic_6dof_joint->get_flag(p_axis,p_flag);
|
||||
}
|
||||
|
||||
|
||||
#if 0
|
||||
void PhysicsServerSW::joint_set_param(RID p_joint, JointParam p_param, real_t p_value) {
|
||||
|
||||
@ -988,12 +1319,18 @@ void PhysicsServerSW::free(RID p_rid) {
|
||||
|
||||
active_spaces.erase(space);
|
||||
free(space->get_default_area()->get_self());
|
||||
free(space->get_static_global_body());
|
||||
|
||||
space_owner.free(p_rid);
|
||||
memdelete(space);
|
||||
} else if (joint_owner.owns(p_rid)) {
|
||||
|
||||
JointSW *joint = joint_owner.get(p_rid);
|
||||
|
||||
for(int i=0;i<joint->get_body_count();i++) {
|
||||
|
||||
joint->get_body_ptr()[i]->remove_constraint(joint);
|
||||
}
|
||||
joint_owner.free(p_rid);
|
||||
memdelete(joint);
|
||||
|
||||
|
||||
@ -125,6 +125,9 @@ public:
|
||||
virtual Variant area_get_param(RID p_parea,AreaParameter p_param) const;
|
||||
virtual Transform area_get_transform(RID p_area) const;
|
||||
|
||||
virtual void area_set_ray_pickable(RID p_area,bool p_enable);
|
||||
virtual bool area_is_ray_pickable(RID p_area) const;
|
||||
|
||||
virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method);
|
||||
|
||||
|
||||
@ -199,6 +202,48 @@ public:
|
||||
virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant());
|
||||
|
||||
/* JOINT API */
|
||||
|
||||
virtual RID joint_create_pin(RID p_body_A,const Vector3& p_local_A,RID p_body_B,const Vector3& p_local_B);
|
||||
|
||||
virtual void pin_joint_set_param(RID p_joint,PinJointParam p_param, float p_value);
|
||||
virtual float pin_joint_get_param(RID p_joint,PinJointParam p_param) const;
|
||||
|
||||
virtual void pin_joint_set_local_A(RID p_joint, const Vector3& p_A);
|
||||
virtual Vector3 pin_joint_get_local_A(RID p_joint) const;
|
||||
|
||||
virtual void pin_joint_set_local_B(RID p_joint, const Vector3& p_B);
|
||||
virtual Vector3 pin_joint_get_local_B(RID p_joint) const;
|
||||
|
||||
virtual RID joint_create_hinge(RID p_body_A,const Transform& p_frame_A,RID p_body_B,const Transform& p_frame_B);
|
||||
virtual RID joint_create_hinge_simple(RID p_body_A,const Vector3& p_pivot_A,const Vector3& p_axis_A,RID p_body_B,const Vector3& p_pivot_B,const Vector3& p_axis_B);
|
||||
|
||||
virtual void hinge_joint_set_param(RID p_joint,HingeJointParam p_param, float p_value);
|
||||
virtual float hinge_joint_get_param(RID p_joint,HingeJointParam p_param) const;
|
||||
|
||||
virtual void hinge_joint_set_flag(RID p_joint,HingeJointFlag p_flag, bool p_value);
|
||||
virtual bool hinge_joint_get_flag(RID p_joint,HingeJointFlag p_flag) const;
|
||||
|
||||
|
||||
virtual RID joint_create_slider(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B); //reference frame is A
|
||||
|
||||
virtual void slider_joint_set_param(RID p_joint,SliderJointParam p_param, float p_value);
|
||||
virtual float slider_joint_get_param(RID p_joint,SliderJointParam p_param) const;
|
||||
|
||||
virtual RID joint_create_cone_twist(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B); //reference frame is A
|
||||
|
||||
virtual void cone_twist_joint_set_param(RID p_joint,ConeTwistJointParam p_param, float p_value);
|
||||
virtual float cone_twist_joint_get_param(RID p_joint,ConeTwistJointParam p_param) const;
|
||||
|
||||
virtual RID joint_create_generic_6dof(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B); //reference frame is A
|
||||
|
||||
virtual void generic_6dof_joint_set_param(RID p_joint,Vector3::Axis,G6DOFJointAxisParam p_param, float p_value);
|
||||
virtual float generic_6dof_joint_get_param(RID p_joint,Vector3::Axis,G6DOFJointAxisParam p_param);
|
||||
|
||||
virtual void generic_6dof_joint_set_flag(RID p_joint,Vector3::Axis,G6DOFJointAxisFlag p_flag, bool p_enable);
|
||||
virtual bool generic_6dof_joint_get_flag(RID p_joint,Vector3::Axis,G6DOFJointAxisFlag p_flag);
|
||||
|
||||
virtual JointType joint_get_type(RID p_joint) const;
|
||||
|
||||
#if 0
|
||||
virtual void joint_set_param(RID p_joint, JointParam p_param, real_t p_value);
|
||||
virtual real_t joint_get_param(RID p_joint,JointParam p_param) const;
|
||||
|
||||
@ -76,6 +76,9 @@ bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vecto
|
||||
if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
|
||||
continue;
|
||||
|
||||
if (space->intersection_query_results[i]->get_type()==CollisionObjectSW::TYPE_AREA && !(static_cast<AreaSW*>(space->intersection_query_results[i])->is_ray_pickable()))
|
||||
continue;
|
||||
|
||||
if (p_exclude.has( space->intersection_query_results[i]->get_self()))
|
||||
continue;
|
||||
|
||||
@ -91,6 +94,7 @@ bool PhysicsDirectSpaceStateSW::intersect_ray(const Vector3& p_from, const Vecto
|
||||
|
||||
Vector3 shape_point,shape_normal;
|
||||
|
||||
|
||||
if (shape->intersect_segment(local_from,local_to,shape_point,shape_normal)) {
|
||||
|
||||
Transform xform = col_obj->get_transform() * col_obj->get_shape_transform(shape_idx);
|
||||
@ -154,6 +158,7 @@ int PhysicsDirectSpaceStateSW::intersect_shape(const RID& p_shape, const Transfo
|
||||
if (!_match_object_type_query(space->intersection_query_results[i],p_layer_mask,p_object_type_mask))
|
||||
continue;
|
||||
|
||||
//area cant be picked by ray (default)
|
||||
|
||||
if (p_exclude.has( space->intersection_query_results[i]->get_self()))
|
||||
continue;
|
||||
|
||||
@ -101,6 +101,8 @@ class SpaceSW {
|
||||
int active_objects;
|
||||
int collision_pairs;
|
||||
|
||||
RID static_global_body;
|
||||
|
||||
friend class PhysicsDirectSpaceStateSW;
|
||||
|
||||
public:
|
||||
@ -164,6 +166,11 @@ public:
|
||||
|
||||
PhysicsDirectSpaceStateSW *get_direct_state();
|
||||
|
||||
|
||||
void set_static_global_body(RID p_body) { static_global_body=p_body; }
|
||||
RID get_static_global_body() { return static_global_body; }
|
||||
|
||||
|
||||
SpaceSW();
|
||||
~SpaceSW();
|
||||
};
|
||||
|
||||
@ -43,6 +43,7 @@ void Area2DSW::set_transform(const Matrix32& p_transform) {
|
||||
get_space()->area_add_to_moved_list(&moved_list);
|
||||
|
||||
_set_transform(p_transform);
|
||||
_set_inv_transform(p_transform.affine_inverse());
|
||||
}
|
||||
|
||||
void Area2DSW::set_space(Space2DSW *p_space) {
|
||||
|
||||
@ -129,11 +129,11 @@ RID PhysicsShapeQueryParameters::get_shape_rid() const {
|
||||
return shape;
|
||||
}
|
||||
|
||||
void PhysicsShapeQueryParameters::set_transform(const Matrix32& p_transform){
|
||||
void PhysicsShapeQueryParameters::set_transform(const Transform& p_transform){
|
||||
|
||||
transform=p_transform;
|
||||
}
|
||||
Matrix32 PhysicsShapeQueryParameters::get_transform() const{
|
||||
Transform PhysicsShapeQueryParameters::get_transform() const{
|
||||
|
||||
return transform;
|
||||
}
|
||||
@ -219,29 +219,8 @@ PhysicsShapeQueryParameters::PhysicsShapeQueryParameters() {
|
||||
|
||||
|
||||
/////////////////////////////////////
|
||||
Variant PhysicsDirectSpaceState::_intersect_ray(const Vector3& p_from, const Vector3& p_to,const Vector<RID>& p_exclude,uint32_t p_user_mask) {
|
||||
|
||||
RayResult inters;
|
||||
Set<RID> exclude;
|
||||
for(int i=0;i<p_exclude.size();i++)
|
||||
exclude.insert(p_exclude[i]);
|
||||
|
||||
bool res = intersect_ray(p_from,p_to,inters,exclude,p_user_mask);
|
||||
|
||||
if (!res)
|
||||
return Variant();
|
||||
|
||||
Dictionary d;
|
||||
d["position"]=inters.position;
|
||||
d["normal"]=inters.normal;
|
||||
d["collider_id"]=inters.collider_id;
|
||||
d["collider"]=inters.collider;
|
||||
d["shape"]=inters.shape;
|
||||
d["rid"]=inters.rid;
|
||||
|
||||
return d;
|
||||
}
|
||||
|
||||
/*
|
||||
Variant PhysicsDirectSpaceState::_intersect_shape(const RID& p_shape, const Transform& p_xform,int p_result_max,const Vector<RID>& p_exclude,uint32_t p_user_mask) {
|
||||
|
||||
|
||||
@ -269,8 +248,98 @@ Variant PhysicsDirectSpaceState::_intersect_shape(const RID& p_shape, const Tran
|
||||
return result;
|
||||
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
Dictionary PhysicsDirectSpaceState::_intersect_ray(const Vector3& p_from, const Vector3& p_to,const Vector<RID>& p_exclude,uint32_t p_layers,uint32_t p_object_type_mask) {
|
||||
|
||||
RayResult inters;
|
||||
Set<RID> exclude;
|
||||
for(int i=0;i<p_exclude.size();i++)
|
||||
exclude.insert(p_exclude[i]);
|
||||
|
||||
bool res = intersect_ray(p_from,p_to,inters,exclude,p_layers,p_object_type_mask);
|
||||
|
||||
if (!res)
|
||||
return Dictionary(true);
|
||||
|
||||
Dictionary d(true);
|
||||
d["position"]=inters.position;
|
||||
d["normal"]=inters.normal;
|
||||
d["collider_id"]=inters.collider_id;
|
||||
d["collider"]=inters.collider;
|
||||
d["shape"]=inters.shape;
|
||||
d["rid"]=inters.rid;
|
||||
|
||||
return d;
|
||||
}
|
||||
|
||||
Array PhysicsDirectSpaceState::_intersect_shape(const Ref<PhysicsShapeQueryParameters> &psq, int p_max_results) {
|
||||
|
||||
Vector<ShapeResult> sr;
|
||||
sr.resize(p_max_results);
|
||||
int rc = intersect_shape(psq->shape,psq->transform,psq->margin,sr.ptr(),sr.size(),psq->exclude,psq->layer_mask,psq->object_type_mask);
|
||||
Array ret;
|
||||
ret.resize(rc);
|
||||
for(int i=0;i<rc;i++) {
|
||||
|
||||
Dictionary d;
|
||||
d["rid"]=sr[i].rid;
|
||||
d["collider_id"]=sr[i].collider_id;
|
||||
d["collider"]=sr[i].collider;
|
||||
d["shape"]=sr[i].shape;
|
||||
ret[i]=d;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
Array PhysicsDirectSpaceState::_cast_motion(const Ref<PhysicsShapeQueryParameters> &psq,const Vector3& p_motion){
|
||||
|
||||
float closest_safe,closest_unsafe;
|
||||
bool res = cast_motion(psq->shape,psq->transform,p_motion,psq->margin,closest_safe,closest_unsafe,psq->exclude,psq->layer_mask,psq->object_type_mask);
|
||||
if (!res)
|
||||
return Array();
|
||||
Array ret(true);
|
||||
ret.resize(2);
|
||||
ret[0]=closest_safe;
|
||||
ret[0]=closest_unsafe;
|
||||
return ret;
|
||||
|
||||
}
|
||||
Array PhysicsDirectSpaceState::_collide_shape(const Ref<PhysicsShapeQueryParameters> &psq, int p_max_results){
|
||||
|
||||
Vector<Vector3> ret;
|
||||
ret.resize(p_max_results*2);
|
||||
int rc=0;
|
||||
bool res = collide_shape(psq->shape,psq->transform,psq->margin,ret.ptr(),p_max_results,rc,psq->exclude,psq->layer_mask,psq->object_type_mask);
|
||||
if (!res)
|
||||
return Array();
|
||||
Array r;
|
||||
r.resize(rc*2);
|
||||
for(int i=0;i<rc*2;i++)
|
||||
r[i]=ret[i];
|
||||
return r;
|
||||
|
||||
}
|
||||
Dictionary PhysicsDirectSpaceState::_get_rest_info(const Ref<PhysicsShapeQueryParameters> &psq){
|
||||
|
||||
ShapeRestInfo sri;
|
||||
|
||||
bool res = rest_info(psq->shape,psq->transform,psq->margin,&sri,psq->exclude,psq->layer_mask,psq->object_type_mask);
|
||||
Dictionary r(true);
|
||||
if (!res)
|
||||
return r;
|
||||
|
||||
r["point"]=sri.point;
|
||||
r["normal"]=sri.normal;
|
||||
r["rid"]=sri.rid;
|
||||
r["collider_id"]=sri.collider_id;
|
||||
r["shape"]=sri.shape;
|
||||
r["linear_velocity"]=sri.linear_velocity;
|
||||
|
||||
return r;
|
||||
}
|
||||
|
||||
|
||||
|
||||
@ -284,8 +353,22 @@ PhysicsDirectSpaceState::PhysicsDirectSpaceState() {
|
||||
void PhysicsDirectSpaceState::_bind_methods() {
|
||||
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("intersect_ray","from","to","exclude","umask"),&PhysicsDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0));
|
||||
ObjectTypeDB::bind_method(_MD("intersect_shape:PhysicsShapeQueryResult","shape","xform","result_max","exclude","umask"),&PhysicsDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0));
|
||||
// ObjectTypeDB::bind_method(_MD("intersect_ray","from","to","exclude","umask"),&PhysicsDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0));
|
||||
// ObjectTypeDB::bind_method(_MD("intersect_shape:PhysicsShapeQueryResult","shape","xform","result_max","exclude","umask"),&PhysicsDirectSpaceState::_intersect_shape,DEFVAL(Array()),DEFVAL(0));
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("intersect_ray:Dictionary","from","to","exclude","layer_mask","type_mask"),&PhysicsDirectSpaceState::_intersect_ray,DEFVAL(Array()),DEFVAL(0x7FFFFFFF),DEFVAL(TYPE_MASK_COLLISION));
|
||||
ObjectTypeDB::bind_method(_MD("intersect_shape","shape:PhysicsShapeQueryParameters","max_results"),&PhysicsDirectSpaceState::_intersect_shape,DEFVAL(32));
|
||||
ObjectTypeDB::bind_method(_MD("cast_motion","shape:PhysicsShapeQueryParameters","motion"),&PhysicsDirectSpaceState::_cast_motion);
|
||||
ObjectTypeDB::bind_method(_MD("collide_shape","shape:PhysicsShapeQueryParameters","max_results"),&PhysicsDirectSpaceState::_collide_shape,DEFVAL(32));
|
||||
ObjectTypeDB::bind_method(_MD("get_rest_info","shape:PhysicsShapeQueryParameters"),&PhysicsDirectSpaceState::_get_rest_info);
|
||||
|
||||
|
||||
BIND_CONSTANT( TYPE_MASK_STATIC_BODY );
|
||||
BIND_CONSTANT( TYPE_MASK_KINEMATIC_BODY );
|
||||
BIND_CONSTANT( TYPE_MASK_RIGID_BODY );
|
||||
BIND_CONSTANT( TYPE_MASK_CHARACTER_BODY );
|
||||
BIND_CONSTANT( TYPE_MASK_AREA );
|
||||
BIND_CONSTANT( TYPE_MASK_COLLISION );
|
||||
|
||||
}
|
||||
|
||||
@ -331,7 +414,6 @@ void PhysicsShapeQueryResult::_bind_methods() {
|
||||
|
||||
|
||||
|
||||
|
||||
///////////////////////////////////////
|
||||
|
||||
void PhysicsServer::_bind_methods() {
|
||||
@ -381,6 +463,9 @@ void PhysicsServer::_bind_methods() {
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("area_set_monitor_callback","receiver","method"),&PhysicsServer::area_set_monitor_callback);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("area_set_ray_pickable","area","enable"),&PhysicsServer::area_set_ray_pickable);
|
||||
ObjectTypeDB::bind_method(_MD("area_is_ray_pickable","area"),&PhysicsServer::area_is_ray_pickable);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("body_create","mode","init_sleeping"),&PhysicsServer::body_create,DEFVAL(BODY_MODE_RIGID),DEFVAL(false));
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("body_set_space","body","space"),&PhysicsServer::body_set_space);
|
||||
@ -436,6 +521,121 @@ void PhysicsServer::_bind_methods() {
|
||||
ObjectTypeDB::bind_method(_MD("body_set_force_integration_callback","body","receiver","method","userdata"),&PhysicsServer::body_set_force_integration_callback,DEFVAL(Variant()));
|
||||
|
||||
/* JOINT API */
|
||||
|
||||
BIND_CONSTANT( JOINT_PIN );
|
||||
BIND_CONSTANT( JOINT_HINGE );
|
||||
BIND_CONSTANT( JOINT_SLIDER );
|
||||
BIND_CONSTANT( JOINT_CONE_TWIST );
|
||||
BIND_CONSTANT( JOINT_6DOF );
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("joint_create_pin","body_A","local_A","body_B","local_B"),&PhysicsServer::joint_create_pin);
|
||||
ObjectTypeDB::bind_method(_MD("pin_joint_set_param","joint","param","value"),&PhysicsServer::pin_joint_set_param);
|
||||
ObjectTypeDB::bind_method(_MD("pin_joint_get_param","joint","param"),&PhysicsServer::pin_joint_get_param);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("pin_joint_set_local_A","joint","local_A"),&PhysicsServer::pin_joint_set_local_A);
|
||||
ObjectTypeDB::bind_method(_MD("pin_joint_get_local_A","joint"),&PhysicsServer::pin_joint_get_local_A);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("pin_joint_set_local_B","joint","local_B"),&PhysicsServer::pin_joint_set_local_B);
|
||||
ObjectTypeDB::bind_method(_MD("pin_joint_get_local_B","joint"),&PhysicsServer::pin_joint_get_local_B);
|
||||
|
||||
BIND_CONSTANT(PIN_JOINT_BIAS );
|
||||
BIND_CONSTANT(PIN_JOINT_DAMPING );
|
||||
BIND_CONSTANT(PIN_JOINT_IMPULSE_CLAMP );
|
||||
|
||||
|
||||
BIND_CONSTANT(HINGE_JOINT_BIAS);
|
||||
BIND_CONSTANT(HINGE_JOINT_LIMIT_UPPER);
|
||||
BIND_CONSTANT(HINGE_JOINT_LIMIT_LOWER);
|
||||
BIND_CONSTANT(HINGE_JOINT_LIMIT_BIAS);
|
||||
BIND_CONSTANT(HINGE_JOINT_LIMIT_SOFTNESS);
|
||||
BIND_CONSTANT(HINGE_JOINT_LIMIT_RELAXATION);
|
||||
BIND_CONSTANT(HINGE_JOINT_MOTOR_TARGET_VELOCITY);
|
||||
BIND_CONSTANT(HINGE_JOINT_MOTOR_MAX_IMPULSE);
|
||||
BIND_CONSTANT(HINGE_JOINT_FLAG_USE_LIMIT);
|
||||
BIND_CONSTANT(HINGE_JOINT_FLAG_ENABLE_MOTOR);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("joint_create_hinge","body_A","hinge_A","body_B","hinge_B"),&PhysicsServer::joint_create_hinge);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("hinge_joint_set_param","joint","param","value"),&PhysicsServer::hinge_joint_set_param);
|
||||
ObjectTypeDB::bind_method(_MD("hinge_joint_get_param","joint","param"),&PhysicsServer::hinge_joint_get_param);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("hinge_joint_set_flag","joint","flag","enabled"),&PhysicsServer::hinge_joint_set_flag);
|
||||
ObjectTypeDB::bind_method(_MD("hinge_joint_get_flag","joint","flag"),&PhysicsServer::hinge_joint_get_flag);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("joint_create_slider","body_A","local_ref_A","body_B","local_ref_B"),&PhysicsServer::joint_create_slider);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("slider_joint_set_param","joint","param","value"),&PhysicsServer::slider_joint_set_param);
|
||||
ObjectTypeDB::bind_method(_MD("slider_joint_get_param","joint","param"),&PhysicsServer::slider_joint_get_param);
|
||||
|
||||
BIND_CONSTANT( SLIDER_JOINT_LINEAR_LIMIT_UPPER );
|
||||
BIND_CONSTANT( SLIDER_JOINT_LINEAR_LIMIT_LOWER );
|
||||
BIND_CONSTANT( SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS );
|
||||
BIND_CONSTANT( SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION );
|
||||
BIND_CONSTANT( SLIDER_JOINT_LINEAR_LIMIT_DAMPING );
|
||||
BIND_CONSTANT( SLIDER_JOINT_LINEAR_MOTION_SOFTNESS );
|
||||
BIND_CONSTANT( SLIDER_JOINT_LINEAR_MOTION_RESTITUTION );
|
||||
BIND_CONSTANT( SLIDER_JOINT_LINEAR_MOTION_DAMPING );
|
||||
BIND_CONSTANT( SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS );
|
||||
BIND_CONSTANT( SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION );
|
||||
BIND_CONSTANT( SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING );
|
||||
|
||||
BIND_CONSTANT( SLIDER_JOINT_ANGULAR_LIMIT_UPPER );
|
||||
BIND_CONSTANT( SLIDER_JOINT_ANGULAR_LIMIT_LOWER );
|
||||
BIND_CONSTANT( SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS );
|
||||
BIND_CONSTANT( SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION );
|
||||
BIND_CONSTANT( SLIDER_JOINT_ANGULAR_LIMIT_DAMPING );
|
||||
BIND_CONSTANT( SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS );
|
||||
BIND_CONSTANT( SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION );
|
||||
BIND_CONSTANT( SLIDER_JOINT_ANGULAR_MOTION_DAMPING );
|
||||
BIND_CONSTANT( SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS );
|
||||
BIND_CONSTANT( SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION );
|
||||
BIND_CONSTANT( SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING );
|
||||
BIND_CONSTANT( SLIDER_JOINT_MAX );
|
||||
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("joint_create_cone_twist","body_A","local_ref_A","body_B","local_ref_B"),&PhysicsServer::joint_create_cone_twist);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("cone_twist_joint_set_param","joint","param","value"),&PhysicsServer::cone_twist_joint_set_param);
|
||||
ObjectTypeDB::bind_method(_MD("cone_twist_joint_get_param","joint","param"),&PhysicsServer::cone_twist_joint_get_param);
|
||||
|
||||
BIND_CONSTANT( CONE_TWIST_JOINT_SWING_SPAN );
|
||||
BIND_CONSTANT( CONE_TWIST_JOINT_TWIST_SPAN );
|
||||
BIND_CONSTANT( CONE_TWIST_JOINT_BIAS );
|
||||
BIND_CONSTANT( CONE_TWIST_JOINT_SOFTNESS );
|
||||
BIND_CONSTANT( CONE_TWIST_JOINT_RELAXATION );
|
||||
|
||||
|
||||
BIND_CONSTANT( G6DOF_JOINT_LINEAR_LOWER_LIMIT );
|
||||
BIND_CONSTANT( G6DOF_JOINT_LINEAR_UPPER_LIMIT );
|
||||
BIND_CONSTANT( G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS );
|
||||
BIND_CONSTANT( G6DOF_JOINT_LINEAR_RESTITUTION );
|
||||
BIND_CONSTANT( G6DOF_JOINT_LINEAR_DAMPING );
|
||||
BIND_CONSTANT( G6DOF_JOINT_ANGULAR_LOWER_LIMIT );
|
||||
BIND_CONSTANT( G6DOF_JOINT_ANGULAR_UPPER_LIMIT );
|
||||
BIND_CONSTANT( G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS );
|
||||
BIND_CONSTANT( G6DOF_JOINT_ANGULAR_DAMPING );
|
||||
BIND_CONSTANT( G6DOF_JOINT_ANGULAR_RESTITUTION );
|
||||
BIND_CONSTANT( G6DOF_JOINT_ANGULAR_FORCE_LIMIT );
|
||||
BIND_CONSTANT( G6DOF_JOINT_ANGULAR_ERP );
|
||||
BIND_CONSTANT( G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY );
|
||||
BIND_CONSTANT( G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT );
|
||||
|
||||
|
||||
BIND_CONSTANT( G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT );
|
||||
BIND_CONSTANT( G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT );
|
||||
BIND_CONSTANT( G6DOF_JOINT_FLAG_ENABLE_MOTOR );
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("joint_get_type","joint"),&PhysicsServer::joint_get_type);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("joint_create_generic_6dof","body_A","local_ref_A","body_B","local_ref_B"),&PhysicsServer::joint_create_generic_6dof);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("generic_6dof_joint_set_param","joint","axis","param","value"),&PhysicsServer::generic_6dof_joint_set_param);
|
||||
ObjectTypeDB::bind_method(_MD("generic_6dof_joint_get_param","joint","axis","param"),&PhysicsServer::generic_6dof_joint_get_param);
|
||||
|
||||
ObjectTypeDB::bind_method(_MD("generic_6dof_joint_set_flag","joint","axis","flag","enable"),&PhysicsServer::generic_6dof_joint_set_flag);
|
||||
ObjectTypeDB::bind_method(_MD("generic_6dof_joint_get_flag","joint","axis","flag"),&PhysicsServer::generic_6dof_joint_get_flag);
|
||||
|
||||
|
||||
/*
|
||||
ObjectTypeDB::bind_method(_MD("joint_set_param","joint","param","value"),&PhysicsServer::joint_set_param);
|
||||
ObjectTypeDB::bind_method(_MD("joint_get_param","joint","param"),&PhysicsServer::joint_get_param);
|
||||
|
||||
@ -92,7 +92,7 @@ class PhysicsShapeQueryParameters : public Reference {
|
||||
OBJ_TYPE(PhysicsShapeQueryParameters, Reference);
|
||||
friend class PhysicsDirectSpaceState;
|
||||
RID shape;
|
||||
Matrix32 transform;
|
||||
Transform transform;
|
||||
float margin;
|
||||
Set<RID> exclude;
|
||||
uint32_t layer_mask;
|
||||
@ -106,8 +106,8 @@ public:
|
||||
void set_shape_rid(const RID& p_shape);
|
||||
RID get_shape_rid() const;
|
||||
|
||||
void set_transform(const Matrix32& p_transform);
|
||||
Matrix32 get_transform() const;
|
||||
void set_transform(const Transform& p_transform);
|
||||
Transform get_transform() const;
|
||||
|
||||
void set_margin(float p_margin);
|
||||
float get_margin() const;
|
||||
@ -131,13 +131,8 @@ class PhysicsDirectSpaceState : public Object {
|
||||
|
||||
OBJ_TYPE( PhysicsDirectSpaceState, Object );
|
||||
|
||||
Variant _intersect_ray(const Vector3& p_from, const Vector3& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_user_mask=0);
|
||||
Variant _intersect_shape(const RID& p_shape, const Transform& p_xform,int p_result_max=64,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_user_mask=0);
|
||||
|
||||
|
||||
protected:
|
||||
static void _bind_methods();
|
||||
|
||||
// Variant _intersect_ray(const Vector3& p_from, const Vector3& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_user_mask=0);
|
||||
// Variant _intersect_shape(const RID& p_shape, const Transform& p_xform,int p_result_max=64,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_user_mask=0);
|
||||
public:
|
||||
|
||||
enum ObjectTypeMask {
|
||||
@ -149,6 +144,21 @@ public:
|
||||
TYPE_MASK_COLLISION=TYPE_MASK_STATIC_BODY|TYPE_MASK_CHARACTER_BODY|TYPE_MASK_KINEMATIC_BODY|TYPE_MASK_RIGID_BODY
|
||||
};
|
||||
|
||||
|
||||
private:
|
||||
Dictionary _intersect_ray(const Vector3& p_from, const Vector3& p_to,const Vector<RID>& p_exclude=Vector<RID>(),uint32_t p_layers=0,uint32_t p_object_type_mask=TYPE_MASK_COLLISION);
|
||||
Array _intersect_shape(const Ref<PhysicsShapeQueryParameters> &p_shape_query,int p_max_results=32);
|
||||
Array _cast_motion(const Ref<PhysicsShapeQueryParameters> &p_shape_query,const Vector3& p_motion);
|
||||
Array _collide_shape(const Ref<PhysicsShapeQueryParameters> &p_shape_query,int p_max_results=32);
|
||||
Dictionary _get_rest_info(const Ref<PhysicsShapeQueryParameters> &p_shape_query);
|
||||
|
||||
|
||||
protected:
|
||||
static void _bind_methods();
|
||||
|
||||
public:
|
||||
|
||||
|
||||
struct RayResult {
|
||||
|
||||
Vector3 position;
|
||||
@ -222,6 +232,7 @@ class PhysicsServer : public Object {
|
||||
|
||||
static PhysicsServer * singleton;
|
||||
|
||||
|
||||
protected:
|
||||
static void _bind_methods();
|
||||
|
||||
@ -329,6 +340,9 @@ public:
|
||||
|
||||
virtual void area_set_monitor_callback(RID p_area,Object *p_receiver,const StringName& p_method)=0;
|
||||
|
||||
virtual void area_set_ray_pickable(RID p_area,bool p_enable)=0;
|
||||
virtual bool area_is_ray_pickable(RID p_area) const=0;
|
||||
|
||||
/* BODY API */
|
||||
|
||||
//missing ccd?
|
||||
@ -437,6 +451,154 @@ public:
|
||||
virtual void body_set_force_integration_callback(RID p_body,Object *p_receiver,const StringName& p_method,const Variant& p_udata=Variant())=0;
|
||||
|
||||
/* JOINT API */
|
||||
|
||||
enum JointType {
|
||||
|
||||
JOINT_PIN,
|
||||
JOINT_HINGE,
|
||||
JOINT_SLIDER,
|
||||
JOINT_CONE_TWIST,
|
||||
JOINT_6DOF
|
||||
|
||||
};
|
||||
|
||||
virtual JointType joint_get_type(RID p_joint) const=0;
|
||||
|
||||
|
||||
|
||||
virtual RID joint_create_pin(RID p_body_A,const Vector3& p_local_A,RID p_body_B,const Vector3& p_local_B)=0;
|
||||
|
||||
enum PinJointParam {
|
||||
PIN_JOINT_BIAS,
|
||||
PIN_JOINT_DAMPING,
|
||||
PIN_JOINT_IMPULSE_CLAMP
|
||||
};
|
||||
|
||||
virtual void pin_joint_set_param(RID p_joint,PinJointParam p_param, float p_value)=0;
|
||||
virtual float pin_joint_get_param(RID p_joint,PinJointParam p_param) const=0;
|
||||
|
||||
virtual void pin_joint_set_local_A(RID p_joint, const Vector3& p_A)=0;
|
||||
virtual Vector3 pin_joint_get_local_A(RID p_joint) const=0;
|
||||
|
||||
virtual void pin_joint_set_local_B(RID p_joint, const Vector3& p_B)=0;
|
||||
virtual Vector3 pin_joint_get_local_B(RID p_joint) const=0;
|
||||
|
||||
enum HingeJointParam {
|
||||
|
||||
HINGE_JOINT_BIAS,
|
||||
HINGE_JOINT_LIMIT_UPPER,
|
||||
HINGE_JOINT_LIMIT_LOWER,
|
||||
HINGE_JOINT_LIMIT_BIAS,
|
||||
HINGE_JOINT_LIMIT_SOFTNESS,
|
||||
HINGE_JOINT_LIMIT_RELAXATION,
|
||||
HINGE_JOINT_MOTOR_TARGET_VELOCITY,
|
||||
HINGE_JOINT_MOTOR_MAX_IMPULSE,
|
||||
HINGE_JOINT_MAX
|
||||
};
|
||||
|
||||
enum HingeJointFlag {
|
||||
HINGE_JOINT_FLAG_USE_LIMIT,
|
||||
HINGE_JOINT_FLAG_ENABLE_MOTOR,
|
||||
HINGE_JOINT_FLAG_MAX
|
||||
};
|
||||
|
||||
virtual RID joint_create_hinge(RID p_body_A,const Transform& p_hinge_A,RID p_body_B,const Transform& p_hinge_B)=0;
|
||||
virtual RID joint_create_hinge_simple(RID p_body_A,const Vector3& p_pivot_A,const Vector3& p_axis_A,RID p_body_B,const Vector3& p_pivot_B,const Vector3& p_axis_B)=0;
|
||||
|
||||
|
||||
virtual void hinge_joint_set_param(RID p_joint,HingeJointParam p_param, float p_value)=0;
|
||||
virtual float hinge_joint_get_param(RID p_joint,HingeJointParam p_param) const=0;
|
||||
|
||||
virtual void hinge_joint_set_flag(RID p_joint,HingeJointFlag p_flag, bool p_value)=0;
|
||||
virtual bool hinge_joint_get_flag(RID p_joint,HingeJointFlag p_flag) const=0;
|
||||
|
||||
enum SliderJointParam {
|
||||
SLIDER_JOINT_LINEAR_LIMIT_UPPER,
|
||||
SLIDER_JOINT_LINEAR_LIMIT_LOWER,
|
||||
SLIDER_JOINT_LINEAR_LIMIT_SOFTNESS,
|
||||
SLIDER_JOINT_LINEAR_LIMIT_RESTITUTION,
|
||||
SLIDER_JOINT_LINEAR_LIMIT_DAMPING,
|
||||
SLIDER_JOINT_LINEAR_MOTION_SOFTNESS,
|
||||
SLIDER_JOINT_LINEAR_MOTION_RESTITUTION,
|
||||
SLIDER_JOINT_LINEAR_MOTION_DAMPING,
|
||||
SLIDER_JOINT_LINEAR_ORTHOGONAL_SOFTNESS,
|
||||
SLIDER_JOINT_LINEAR_ORTHOGONAL_RESTITUTION,
|
||||
SLIDER_JOINT_LINEAR_ORTHOGONAL_DAMPING,
|
||||
|
||||
SLIDER_JOINT_ANGULAR_LIMIT_UPPER,
|
||||
SLIDER_JOINT_ANGULAR_LIMIT_LOWER,
|
||||
SLIDER_JOINT_ANGULAR_LIMIT_SOFTNESS,
|
||||
SLIDER_JOINT_ANGULAR_LIMIT_RESTITUTION,
|
||||
SLIDER_JOINT_ANGULAR_LIMIT_DAMPING,
|
||||
SLIDER_JOINT_ANGULAR_MOTION_SOFTNESS,
|
||||
SLIDER_JOINT_ANGULAR_MOTION_RESTITUTION,
|
||||
SLIDER_JOINT_ANGULAR_MOTION_DAMPING,
|
||||
SLIDER_JOINT_ANGULAR_ORTHOGONAL_SOFTNESS,
|
||||
SLIDER_JOINT_ANGULAR_ORTHOGONAL_RESTITUTION,
|
||||
SLIDER_JOINT_ANGULAR_ORTHOGONAL_DAMPING,
|
||||
SLIDER_JOINT_MAX
|
||||
|
||||
|
||||
};
|
||||
|
||||
virtual RID joint_create_slider(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B)=0; //reference frame is A
|
||||
|
||||
virtual void slider_joint_set_param(RID p_joint,SliderJointParam p_param, float p_value)=0;
|
||||
virtual float slider_joint_get_param(RID p_joint,SliderJointParam p_param) const=0;
|
||||
|
||||
enum ConeTwistJointParam {
|
||||
CONE_TWIST_JOINT_SWING_SPAN,
|
||||
CONE_TWIST_JOINT_TWIST_SPAN,
|
||||
CONE_TWIST_JOINT_BIAS,
|
||||
CONE_TWIST_JOINT_SOFTNESS,
|
||||
CONE_TWIST_JOINT_RELAXATION,
|
||||
CONE_TWIST_MAX
|
||||
};
|
||||
|
||||
|
||||
virtual RID joint_create_cone_twist(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B)=0; //reference frame is A
|
||||
|
||||
virtual void cone_twist_joint_set_param(RID p_joint,ConeTwistJointParam p_param, float p_value)=0;
|
||||
virtual float cone_twist_joint_get_param(RID p_joint,ConeTwistJointParam p_param) const=0;
|
||||
|
||||
|
||||
enum G6DOFJointAxisParam {
|
||||
G6DOF_JOINT_LINEAR_LOWER_LIMIT,
|
||||
G6DOF_JOINT_LINEAR_UPPER_LIMIT,
|
||||
G6DOF_JOINT_LINEAR_LIMIT_SOFTNESS,
|
||||
G6DOF_JOINT_LINEAR_RESTITUTION,
|
||||
G6DOF_JOINT_LINEAR_DAMPING,
|
||||
G6DOF_JOINT_ANGULAR_LOWER_LIMIT,
|
||||
G6DOF_JOINT_ANGULAR_UPPER_LIMIT,
|
||||
G6DOF_JOINT_ANGULAR_LIMIT_SOFTNESS,
|
||||
G6DOF_JOINT_ANGULAR_DAMPING,
|
||||
G6DOF_JOINT_ANGULAR_RESTITUTION,
|
||||
G6DOF_JOINT_ANGULAR_FORCE_LIMIT,
|
||||
G6DOF_JOINT_ANGULAR_ERP,
|
||||
G6DOF_JOINT_ANGULAR_MOTOR_TARGET_VELOCITY,
|
||||
G6DOF_JOINT_ANGULAR_MOTOR_FORCE_LIMIT,
|
||||
G6DOF_JOINT_MAX
|
||||
};
|
||||
|
||||
enum G6DOFJointAxisFlag {
|
||||
|
||||
G6DOF_JOINT_FLAG_ENABLE_LINEAR_LIMIT,
|
||||
G6DOF_JOINT_FLAG_ENABLE_ANGULAR_LIMIT,
|
||||
G6DOF_JOINT_FLAG_ENABLE_MOTOR,
|
||||
G6DOF_JOINT_FLAG_MAX
|
||||
};
|
||||
|
||||
|
||||
|
||||
virtual RID joint_create_generic_6dof(RID p_body_A,const Transform& p_local_frame_A,RID p_body_B,const Transform& p_local_frame_B)=0; //reference frame is A
|
||||
|
||||
virtual void generic_6dof_joint_set_param(RID p_joint,Vector3::Axis,G6DOFJointAxisParam p_param, float p_value)=0;
|
||||
virtual float generic_6dof_joint_get_param(RID p_joint,Vector3::Axis,G6DOFJointAxisParam p_param)=0;
|
||||
|
||||
virtual void generic_6dof_joint_set_flag(RID p_joint,Vector3::Axis,G6DOFJointAxisFlag p_flag, bool p_enable)=0;
|
||||
virtual bool generic_6dof_joint_get_flag(RID p_joint,Vector3::Axis,G6DOFJointAxisFlag p_flag)=0;
|
||||
|
||||
|
||||
#if 0
|
||||
enum JointType {
|
||||
|
||||
@ -508,9 +670,14 @@ VARIANT_ENUM_CAST( PhysicsServer::BodyMode );
|
||||
VARIANT_ENUM_CAST( PhysicsServer::BodyParameter );
|
||||
VARIANT_ENUM_CAST( PhysicsServer::BodyState );
|
||||
VARIANT_ENUM_CAST( PhysicsServer::BodyAxisLock );
|
||||
//VARIANT_ENUM_CAST( PhysicsServer::JointParam );
|
||||
//VARIANT_ENUM_CAST( PhysicsServer::JointType );
|
||||
//VARIANT_ENUM_CAST( PhysicsServer::DampedStringParam );
|
||||
VARIANT_ENUM_CAST( PhysicsServer::PinJointParam );
|
||||
VARIANT_ENUM_CAST( PhysicsServer::JointType );
|
||||
VARIANT_ENUM_CAST( PhysicsServer::HingeJointParam );
|
||||
VARIANT_ENUM_CAST( PhysicsServer::HingeJointFlag );
|
||||
VARIANT_ENUM_CAST( PhysicsServer::SliderJointParam );
|
||||
VARIANT_ENUM_CAST( PhysicsServer::ConeTwistJointParam );
|
||||
VARIANT_ENUM_CAST( PhysicsServer::G6DOFJointAxisParam );
|
||||
VARIANT_ENUM_CAST( PhysicsServer::G6DOFJointAxisFlag);
|
||||
//VARIANT_ENUM_CAST( PhysicsServer::ObjectType );
|
||||
VARIANT_ENUM_CAST( PhysicsServer::AreaBodyStatus );
|
||||
VARIANT_ENUM_CAST( PhysicsServer::ProcessInfo );
|
||||
|
||||
Reference in New Issue
Block a user