25 #define CONETWIST_USE_OBSOLETE_SOLVER false 26 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f) 110 calcAngleInfo2(transA, transB, invInertiaWorldA, invInertiaWorldB);
139 for (j = 0; j < 3; j++)
150 int srow = row * info->
rowskip;
162 int srow1 = srow + info->
rowskip;
166 J1[srow1 + 0] = q[0];
167 J1[srow1 + 1] = q[1];
168 J1[srow1 + 2] = q[2];
169 J2[srow + 0] = -p[0];
170 J2[srow + 1] = -p[1];
171 J2[srow + 2] = -p[2];
172 J2[srow1 + 0] = -q[0];
173 J2[srow1 + 1] = -q[1];
174 J2[srow1 + 2] = -q[2];
187 J1[srow + 0] = ax1[0];
188 J1[srow + 1] = ax1[1];
189 J1[srow + 2] = ax1[2];
190 J2[srow + 0] = -ax1[0];
191 J2[srow + 1] = -ax1[1];
192 J2[srow + 2] = -ax1[2];
211 J1[srow + 0] = ax1[0];
212 J1[srow + 1] = ax1[1];
213 J1[srow + 2] = ax1[2];
214 J2[srow + 0] = -ax1[0];
215 J2[srow + 1] = -ax1[1];
216 J2[srow + 2] = -ax1[2];
258 btVector3 relPos = pivotBInW - pivotAInW;
272 for (
int i = 0; i < 3; i++)
313 for (
int i = 0; i < 3; i++)
319 rel_vel = normal.
dot(vel);
321 btScalar depth = -(pivotAInW - pivotBInW).
dot(normal);
322 btScalar impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
346 trACur, zerovec, omegaA, timeStep, trAPred);
350 trBCur, zerovec, omegaB, timeStep, trBPred);
370 btScalar kAxisAInv = 0, kAxisBInv = 0;
384 btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
386 static bool bDoTorque =
true;
392 btScalar kInvCombined = kAxisAInv + kAxisBInv;
394 btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
395 (kInvCombined * kInvCombined);
401 fMaxImpulse = fMaxImpulse / kAxisAInv;
405 if (newUnclampedMag > fMaxImpulse)
408 newUnclampedAccImpulse *= fMaxImpulse;
415 btVector3 impulseAxis = impulse / impulseMag;
437 btVector3 impulseAxis = impulse / impulseMag;
471 btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
472 impulse = impulseNoTwistCouple;
475 impulseMag = impulse.
length();
476 btVector3 noTwistSwingAxis = impulse / impulseMag;
521 btVector3 b1Axis1(0, 0, 0), b1Axis2(0, 0, 0), b1Axis3(0, 0, 0);
522 btVector3 b2Axis1(0, 0, 0), b2Axis2(0, 0, 0);
537 swx = b2Axis1.
dot(b1Axis1);
538 swy = b2Axis1.dot(b1Axis2);
540 fact = (swy * swy + swx * swx) * thresh * thresh;
541 fact = fact / (fact +
btScalar(1.0));
548 swx = b2Axis1.
dot(b1Axis1);
549 swy = b2Axis1.dot(b1Axis3);
551 fact = (swy * swy + swx * swx) * thresh * thresh;
552 fact = fact / (fact +
btScalar(1.0));
558 btScalar EllipseAngle =
btFabs(swing1 * swing1) * RMaxAngle1Sq +
btFabs(swing2 * swing2) * RMaxAngle2Sq;
560 if (EllipseAngle > 1.0f)
565 m_swingAxis = b2Axis1.
cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.
dot(b1Axis3));
567 btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
652 btScalar swingAngle, swingLimit = 0;
755 target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
756 target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
757 target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
814 vSwingAxis =
btVector3(qCone.
x(), qCone.
y(), qCone.
z());
841 btScalar surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse);
844 btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
845 swingLimit = sqrt(swingLimit2);
861 else if (swingAngle < 0)
886 btScalar surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse);
889 btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
890 swingLimit = sqrt(swingLimit2);
895 btVector3 vSwingAxis(0, xEllipse, -yEllipse);
897 btVector3 vPointInConstraintSpace(fLength, 0, 0);
898 return quatRotate(qSwing, vPointInConstraintSpace);
912 qMinTwist = -(qTwist);
923 vTwistAxis =
btVector3(qMinTwist.
x(), qMinTwist.
y(), qMinTwist.
z());
997 if (swingAngle > swingLimit * softness)
998 swingAngle = swingLimit * softness;
999 else if (swingAngle < -swingLimit * softness)
1000 swingAngle = -swingLimit * softness;
1035 if ((axis >= 0) && (axis < 3))
1047 if ((axis >= 0) && (axis < 3))
1072 if ((axis >= 0) && (axis < 3))
1077 else if ((axis >= 3) && (axis < 6))
1088 if ((axis >= 0) && (axis < 3))
1093 else if ((axis >= 3) && (axis < 6))
btScalar * m_constraintError
const btScalar & x() const
Return the x value.
const btRigidBody & getRigidBodyA() const
void setLimit(int limitIndex, btScalar limitValue)
btScalar computeAngularImpulseDenominator(const btVector3 &axis) const
btScalar m_maxMotorImpulse
btScalar computeAngularImpulseDenominator(const btVector3 &axis, const btMatrix3x3 &invInertiaWorld)
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
btScalar * m_J2angularAxis
void setMotorTarget(const btQuaternion &q)
btScalar m_swingCorrection
static btVector3 vTwist(1, 0, 0)
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
#define CONETWIST_DEF_FIX_THRESH
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
void internalGetAngularVelocity(btVector3 &angVel) const
btScalar btSin(btScalar x)
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
const btTransform & getCenterOfMassTransform() const
#define CONETWIST_USE_OBSOLETE_SOLVER
btScalar length2() const
Return the length of the vector squared.
void setZ(btScalar _z)
Set the z value.
const btRigidBody & getRigidBodyB() const
virtual void setParam(int num, btScalar value, int axis=-1)
override the default global value of a parameter (such as ERP or CFM), optionally provide the axis (0...
void btPlaneSpace1(const T &n, T &p, T &q)
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
#define SIMD_FORCE_INLINE
btScalar * m_J1angularAxis
btScalar m_twistLimitRatio
btScalar m_relaxationFactor
btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
void setMotorTargetInConstraintSpace(const btQuaternion &q)
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
btVector3 normalized() const
Return a normalized version of this vector.
btMatrix3x3 transpose() const
Return the transpose of the matrix.
void updateRHS(btScalar timeStep)
btQuaternion inverse() const
Return the inverse of this quaternion.
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
btScalar m_twistLimitSign
void adjustSwingAxisToUseEllipseNormal(btVector3 &vSwingAxis) const
btVector3 m_linearJointAxis
const btVector3 & getInvInertiaDiagLocal() const
btScalar * m_J1linearAxis
btQuaternion shortestArcQuat(const btVector3 &v0, const btVector3 &v1)
btConeTwistConstraint(btRigidBody &rbA, btRigidBody &rbB, const btTransform &rbAFrame, const btTransform &rbBFrame)
const btScalar & x() const
Return the x value.
virtual void solveConstraintObsolete(btSolverBody &bodyA, btSolverBody &bodyB, btScalar timeStep)
internal method used by the constraint solver, don't use them directly
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
btScalar getInvMass() const
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
void getSkewSymmetricMatrix(btVector3 *v0, btVector3 *v1, btVector3 *v2) const
btScalar dot(const btVector3 &v) const
Return the dot product.
btScalar btAtan2Fast(btScalar y, btScalar x)
btScalar btAtan2(btScalar x, btScalar y)
btVector3 m_accMotorImpulse
void internalGetVelocityInLocalPointObsolete(const btVector3 &rel_pos, btVector3 &velocity) const
void setY(btScalar _y)
Set the y value.
const btScalar & y() const
Return the y value.
const btScalar & z() const
Return the z value.
The btRigidBody is the main class for rigid body objects.
bool m_useSolveConstraintObsolete
void getInfo2NonVirtual(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btMatrix3x3 &invInertiaWorldA, const btMatrix3x3 &invInertiaWorldB)
const btScalar & z() const
Return the z value.
btScalar m_twistCorrection
btVector3 can be used to represent 3D points and vectors.
btScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
const btVector3 & getCenterOfMassPosition() const
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
void computeConeLimitInfo(const btQuaternion &qCone, btScalar &swingAngle, btVector3 &vSwingAxis, btScalar &swingLimit)
btScalar * m_J2linearAxis
btScalar getDiagonal() const
TypedConstraint is the baseclass for Bullet constraints and vehicles.
bool btFuzzyZero(btScalar x)
void calcAngleInfo2(const btTransform &transA, const btTransform &transB, const btMatrix3x3 &invInertiaWorldA, const btMatrix3x3 &invInertiaWorldB)
virtual void setFrames(const btTransform &frameA, const btTransform &frameB)
btScalar m_accSwingLimitImpulse
const T & btMax(const T &a, const T &b)
void getInfo1NonVirtual(btConstraintInfo1 *info)
btScalar m_appliedImpulse
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
const btScalar & y() const
Return the y value.
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don't use them directly
btScalar m_swingLimitRatio
const btMatrix3x3 & getInvInertiaTensorWorld() const
btScalar m_accTwistLimitImpulse
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
#define btAssertConstrParams(_par)
bool m_bNormalizedMotorStrength
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
void computeTwistLimitInfo(const btQuaternion &qTwist, btScalar &twistAngle, btVector3 &vTwistAxis)
btScalar btCos(btScalar x)
btScalar length() const
Return the length of the vector.
btScalar btFabs(btScalar x)