summaryrefslogtreecommitdiff
path: root/servers/physics/joints
diff options
context:
space:
mode:
Diffstat (limited to 'servers/physics/joints')
-rw-r--r--servers/physics/joints/cone_twist_joint_sw.cpp14
-rw-r--r--servers/physics/joints/cone_twist_joint_sw.h2
-rw-r--r--servers/physics/joints/generic_6dof_joint_sw.cpp55
-rw-r--r--servers/physics/joints/generic_6dof_joint_sw.h2
-rw-r--r--servers/physics/joints/hinge_joint_sw.cpp34
-rw-r--r--servers/physics/joints/hinge_joint_sw.h2
-rw-r--r--servers/physics/joints/jacobian_entry_sw.h12
-rw-r--r--servers/physics/joints/pin_joint_sw.cpp14
-rw-r--r--servers/physics/joints/pin_joint_sw.h2
-rw-r--r--servers/physics/joints/slider_joint_sw.cpp14
-rw-r--r--servers/physics/joints/slider_joint_sw.h2
11 files changed, 77 insertions, 76 deletions
diff --git a/servers/physics/joints/cone_twist_joint_sw.cpp b/servers/physics/joints/cone_twist_joint_sw.cpp
index 5f1dde4e20..cbf444f1e8 100644
--- a/servers/physics/joints/cone_twist_joint_sw.cpp
+++ b/servers/physics/joints/cone_twist_joint_sw.cpp
@@ -5,7 +5,7 @@
/* GODOT ENGINE */
/* http://www.godotengine.org */
/*************************************************************************/
-/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
@@ -128,10 +128,10 @@ bool ConeTwistJointSW::setup(float p_step) {
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,
+ A->get_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
+ pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+ pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
normal[i],
A->get_inv_inertia(),
A->get_inv_mass(),
@@ -156,7 +156,7 @@ bool ConeTwistJointSW::setup(float p_step) {
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) );
+ //swing1 = btAtan2Fast( b2Axis1.dot(b1Axis2),b2Axis1.dot(b1Axis1) );
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis2);
swing1 = atan2fast(swy, swx);
@@ -169,7 +169,7 @@ bool ConeTwistJointSW::setup(float p_step) {
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) );
+ //swing2 = btAtan2Fast( b2Axis1.dot(b1Axis3),b2Axis1.dot(b1Axis1) );
swx = b2Axis1.dot(b1Axis1);
swy = b2Axis1.dot(b1Axis3);
swing2 = atan2fast(swy, swx);
diff --git a/servers/physics/joints/cone_twist_joint_sw.h b/servers/physics/joints/cone_twist_joint_sw.h
index 653259071d..0d64d67443 100644
--- a/servers/physics/joints/cone_twist_joint_sw.h
+++ b/servers/physics/joints/cone_twist_joint_sw.h
@@ -5,7 +5,7 @@
/* GODOT ENGINE */
/* http://www.godotengine.org */
/*************************************************************************/
-/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
diff --git a/servers/physics/joints/generic_6dof_joint_sw.cpp b/servers/physics/joints/generic_6dof_joint_sw.cpp
index 06015a5228..48f70d9077 100644
--- a/servers/physics/joints/generic_6dof_joint_sw.cpp
+++ b/servers/physics/joints/generic_6dof_joint_sw.cpp
@@ -5,7 +5,7 @@
/* GODOT ENGINE */
/* http://www.godotengine.org */
/*************************************************************************/
-/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
@@ -38,8 +38,8 @@ See corresponding header file for licensing info.
#define GENERIC_D6_DISABLE_WARMSTARTING 1
-real_t btGetMatrixElem(const Matrix3& mat, int index);
-real_t btGetMatrixElem(const Matrix3& mat, int index)
+real_t btGetMatrixElem(const Basis& mat, int index);
+real_t btGetMatrixElem(const Basis& mat, int index)
{
int i = index%3;
int j = index/3;
@@ -47,13 +47,12 @@ real_t btGetMatrixElem(const Matrix3& mat, int index)
}
///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)
+bool matrixToEulerXYZ(const Basis& mat,Vector3& xyz);
+bool matrixToEulerXYZ(const Basis& 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
-//
+ // 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))
{
@@ -296,7 +295,7 @@ Generic6DOFJointSW::Generic6DOFJointSW(BodySW* rbA, BodySW* rbB, const Transform
void Generic6DOFJointSW::calculateAngleInfo()
{
- Matrix3 relative_frame = m_calculatedTransformA.basis.inverse()*m_calculatedTransformB.basis;
+ Basis relative_frame = m_calculatedTransformA.basis.inverse()*m_calculatedTransformB.basis;
matrixToEulerXYZ(relative_frame,m_calculatedAxisAngleDiff);
@@ -325,16 +324,18 @@ void Generic6DOFJointSW::calculateAngleInfo()
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);
-// }
+ /*
+ 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);
+ }
+ */
}
@@ -352,10 +353,10 @@ void Generic6DOFJointSW::buildLinearJacobian(
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,
+ A->get_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
+ pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+ pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
normalWorld,
A->get_inv_inertia(),
A->get_inv_mass(),
@@ -368,8 +369,8 @@ 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_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
A->get_inv_inertia(),
B->get_inv_inertia()));
@@ -440,7 +441,7 @@ bool Generic6DOFJointSW::setup(float p_step) {
}
-void Generic6DOFJointSW::solve(real_t timeStep)
+void Generic6DOFJointSW::solve(real_t timeStep)
{
m_timeStep = timeStep;
diff --git a/servers/physics/joints/generic_6dof_joint_sw.h b/servers/physics/joints/generic_6dof_joint_sw.h
index 47ef43156d..4ac727c124 100644
--- a/servers/physics/joints/generic_6dof_joint_sw.h
+++ b/servers/physics/joints/generic_6dof_joint_sw.h
@@ -5,7 +5,7 @@
/* GODOT ENGINE */
/* http://www.godotengine.org */
/*************************************************************************/
-/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
diff --git a/servers/physics/joints/hinge_joint_sw.cpp b/servers/physics/joints/hinge_joint_sw.cpp
index 035407065c..277346fbbb 100644
--- a/servers/physics/joints/hinge_joint_sw.cpp
+++ b/servers/physics/joints/hinge_joint_sw.cpp
@@ -5,7 +5,7 @@
/* GODOT ENGINE */
/* http://www.godotengine.org */
/*************************************************************************/
-/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
@@ -112,7 +112,7 @@ HingeJointSW::HingeJointSW(BodySW* rbA,BodySW* rbB, const Vector3& pivotInA,cons
rbAxisA1 = rbAxisA2.cross(axisInA);
}
- m_rbAFrame.basis=Matrix3( rbAxisA1.x,rbAxisA2.x,axisInA.x,
+ m_rbAFrame.basis=Basis( rbAxisA1.x,rbAxisA2.x,axisInA.x,
rbAxisA1.y,rbAxisA2.y,axisInA.y,
rbAxisA1.z,rbAxisA2.z,axisInA.z );
@@ -121,7 +121,7 @@ HingeJointSW::HingeJointSW(BodySW* rbA,BodySW* rbB, const Vector3& pivotInA,cons
Vector3 rbAxisB2 = axisInB.cross(rbAxisB1);
m_rbBFrame.origin = pivotInB;
- m_rbBFrame.basis=Matrix3( rbAxisB1.x,rbAxisB2.x,-axisInB.x,
+ m_rbBFrame.basis=Basis( rbAxisB1.x,rbAxisB2.x,-axisInB.x,
rbAxisB1.y,rbAxisB2.y,-axisInB.y,
rbAxisB1.z,rbAxisB2.z,-axisInB.z );
@@ -173,10 +173,10 @@ bool HingeJointSW::setup(float p_step) {
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,
+ A->get_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
+ pivotAInW - A->get_transform().origin - A->get_center_of_mass(),
+ pivotBInW - B->get_transform().origin - B->get_center_of_mass(),
normal[i],
A->get_inv_inertia(),
A->get_inv_mass(),
@@ -200,20 +200,20 @@ bool HingeJointSW::setup(float p_step) {
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_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().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_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().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_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
A->get_inv_inertia(),
B->get_inv_inertia()));
@@ -221,7 +221,7 @@ bool HingeJointSW::setup(float p_step) {
// Compute limit information
real_t hingeAngle = get_hinge_angle();
-// print_line("angle: "+rtos(hingeAngle));
+ //print_line("angle: "+rtos(hingeAngle));
//set bias, sign, clear accumulator
m_correction = real_t(0.);
m_limitSign = real_t(0.);
@@ -235,17 +235,17 @@ bool HingeJointSW::setup(float p_step) {
print_line("hi: "+rtos(m_upperLimit));
}*/
-// if (m_lowerLimit < 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_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_limitSoftness)
else if (hingeAngle >= m_upperLimit)
{
m_correction = m_upperLimit - hingeAngle;
diff --git a/servers/physics/joints/hinge_joint_sw.h b/servers/physics/joints/hinge_joint_sw.h
index f87c2ac4c5..60203e3cc4 100644
--- a/servers/physics/joints/hinge_joint_sw.h
+++ b/servers/physics/joints/hinge_joint_sw.h
@@ -5,7 +5,7 @@
/* GODOT ENGINE */
/* http://www.godotengine.org */
/*************************************************************************/
-/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
diff --git a/servers/physics/joints/jacobian_entry_sw.h b/servers/physics/joints/jacobian_entry_sw.h
index b7ab58f16b..cd85162ba5 100644
--- a/servers/physics/joints/jacobian_entry_sw.h
+++ b/servers/physics/joints/jacobian_entry_sw.h
@@ -5,7 +5,7 @@
/* GODOT ENGINE */
/* http://www.godotengine.org */
/*************************************************************************/
-/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
@@ -56,8 +56,8 @@ public:
JacobianEntrySW() {};
//constraint between two different rigidbodies
JacobianEntrySW(
- const Matrix3& world2A,
- const Matrix3& world2B,
+ const Basis& world2A,
+ const Basis& world2B,
const Vector3& rel_pos1,const Vector3& rel_pos2,
const Vector3& jointAxis,
const Vector3& inertiaInvA,
@@ -77,8 +77,8 @@ public:
//angular constraint between two different rigidbodies
JacobianEntrySW(const Vector3& jointAxis,
- const Matrix3& world2A,
- const Matrix3& world2B,
+ const Basis& world2A,
+ const Basis& world2B,
const Vector3& inertiaInvA,
const Vector3& inertiaInvB)
:m_linearJointAxis(Vector3(real_t(0.),real_t(0.),real_t(0.)))
@@ -110,7 +110,7 @@ public:
//constraint on one rigidbody
JacobianEntrySW(
- const Matrix3& world2A,
+ const Basis& world2A,
const Vector3& rel_pos1,const Vector3& rel_pos2,
const Vector3& jointAxis,
const Vector3& inertiaInvA,
diff --git a/servers/physics/joints/pin_joint_sw.cpp b/servers/physics/joints/pin_joint_sw.cpp
index 013d750b4f..9c7fe65684 100644
--- a/servers/physics/joints/pin_joint_sw.cpp
+++ b/servers/physics/joints/pin_joint_sw.cpp
@@ -5,7 +5,7 @@
/* GODOT ENGINE */
/* http://www.godotengine.org */
/*************************************************************************/
-/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
@@ -44,10 +44,10 @@ bool PinJointSW::setup(float p_step) {
{
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,
+ A->get_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
+ A->get_transform().xform(m_pivotInA) - A->get_transform().origin - A->get_center_of_mass(),
+ B->get_transform().xform(m_pivotInB) - B->get_transform().origin - B->get_center_of_mass(),
normal,
A->get_inv_inertia(),
A->get_inv_mass(),
@@ -68,8 +68,8 @@ void PinJointSW::solve(float p_step){
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();
+ //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++)
{
diff --git a/servers/physics/joints/pin_joint_sw.h b/servers/physics/joints/pin_joint_sw.h
index 4ef134fe73..6797972496 100644
--- a/servers/physics/joints/pin_joint_sw.h
+++ b/servers/physics/joints/pin_joint_sw.h
@@ -5,7 +5,7 @@
/* GODOT ENGINE */
/* http://www.godotengine.org */
/*************************************************************************/
-/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
diff --git a/servers/physics/joints/slider_joint_sw.cpp b/servers/physics/joints/slider_joint_sw.cpp
index a9072e5de3..bdcae08ca4 100644
--- a/servers/physics/joints/slider_joint_sw.cpp
+++ b/servers/physics/joints/slider_joint_sw.cpp
@@ -5,7 +5,7 @@
/* GODOT ENGINE */
/* http://www.godotengine.org */
/*************************************************************************/
-/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
@@ -132,10 +132,10 @@ bool SliderJointSW::setup(float p_step)
{
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,
+ A->get_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
+ m_relPosA - A->get_center_of_mass(),
+ m_relPosB - B->get_center_of_mass(),
normalWorld,
A->get_inv_inertia(),
A->get_inv_mass(),
@@ -152,8 +152,8 @@ bool SliderJointSW::setup(float p_step)
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_principal_inertia_axes().transposed(),
+ B->get_principal_inertia_axes().transposed(),
A->get_inv_inertia(),
B->get_inv_inertia()
));
diff --git a/servers/physics/joints/slider_joint_sw.h b/servers/physics/joints/slider_joint_sw.h
index 9ee6c83800..4e697e6f64 100644
--- a/servers/physics/joints/slider_joint_sw.h
+++ b/servers/physics/joints/slider_joint_sw.h
@@ -5,7 +5,7 @@
/* GODOT ENGINE */
/* http://www.godotengine.org */
/*************************************************************************/
-/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */
+/* Copyright (c) 2007-2017 Juan Linietsky, Ariel Manzur. */
/* */
/* Permission is hereby granted, free of charge, to any person obtaining */
/* a copy of this software and associated documentation files (the */
generated by cgit v1.2.3 (git 2.25.1) at 2025-02-17 22:44:28 +0000