28#define CONETWIST_USE_OBSOLETE_SOLVER false
29#define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
165 int srow = row * info->
rowskip;
177 int srow1 = srow + info->
rowskip;
205 J2[srow+0] = -ax1[0];
206 J2[srow+1] = -ax1[1];
207 J2[srow+2] = -ax1[2];
229 J2[srow+0] = -ax1[0];
230 J2[srow+1] = -ax1[1];
231 J2[srow+2] = -ax1[2];
276 btVector3 relPos = pivotBInW - pivotAInW;
290 for (
int i=0;i<3;i++)
333 for (
int i=0;i<3;i++)
339 rel_vel = normal.
dot(vel);
341 btScalar depth = -(pivotAInW - pivotBInW).
dot(normal);
342 btScalar impulse = depth*tau/timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
364 trACur, zerovec, omegaA, timeStep, trAPred);
367 trBCur, zerovec, omegaB, timeStep, trBPred);
387 btScalar kAxisAInv = 0, kAxisBInv = 0;
401 btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
403 static bool bDoTorque =
true;
409 btScalar kInvCombined = kAxisAInv + kAxisBInv;
411 btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
412 (kInvCombined * kInvCombined);
418 fMaxImpulse = fMaxImpulse/kAxisAInv;
422 if (newUnclampedMag > fMaxImpulse)
425 newUnclampedAccImpulse *= fMaxImpulse;
432 btVector3 impulseAxis = impulse / impulseMag;
453 btVector3 impulseAxis = impulse / impulseMag;
487 btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
488 impulse = impulseNoTwistCouple;
491 impulseMag = impulse.
length();
492 btVector3 noTwistSwingAxis = impulse / impulseMag;
543 btVector3 b1Axis1(0,0,0),b1Axis2(0,0,0),b1Axis3(0,0,0);
559 swx = b2Axis1.
dot(b1Axis1);
560 swy = b2Axis1.dot(b1Axis2);
562 fact = (swy*swy + swx*swx) * thresh * thresh;
563 fact = fact / (fact +
btScalar(1.0));
570 swx = b2Axis1.
dot(b1Axis1);
571 swy = b2Axis1.dot(b1Axis3);
573 fact = (swy*swy + swx*swx) * thresh * thresh;
574 fact = fact / (fact +
btScalar(1.0));
580 btScalar EllipseAngle =
btFabs(swing1*swing1)* RMaxAngle1Sq +
btFabs(swing2*swing2) * RMaxAngle2Sq;
582 if (EllipseAngle > 1.0f)
587 m_swingAxis = b2Axis1.
cross(b1Axis2* b2Axis1.dot(b1Axis2) + b1Axis3* b2Axis1.
dot(b1Axis3));
589 btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
776 target[0] = x * ivA[0] + y * jvA[0] + z * kvA[0];
777 target[1] = x * ivA[1] + y * jvA[1] + z * kvA[1];
778 target[2] = x * ivA[2] + y * jvA[2] + z * kvA[2];
837 vSwingAxis =
btVector3(qCone.
x(), qCone.
y(), qCone.
z());
864 btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
867 btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
868 swingLimit = sqrt(swingLimit2);
884 else if (swingAngle < 0)
909 btScalar surfaceSlope2 = (yEllipse*yEllipse)/(xEllipse*xEllipse);
912 btScalar swingLimit2 = (1 + surfaceSlope2) / norm;
913 swingLimit = sqrt(swingLimit2);
918 btVector3 vSwingAxis(0, xEllipse, -yEllipse);
920 btVector3 vPointInConstraintSpace(fLength,0,0);
921 return quatRotate(qSwing, vPointInConstraintSpace);
935 qMinTwist = -(qTwist);
946 vTwistAxis =
btVector3(qMinTwist.
x(), qMinTwist.
y(), qMinTwist.
z());
1021 if (swingAngle > swingLimit*softness)
1022 swingAngle = swingLimit*softness;
1023 else if (swingAngle < -swingLimit*softness)
1024 swingAngle = -swingLimit*softness;
1058 if((axis >= 0) && (axis < 3))
1070 if((axis >= 0) && (axis < 3))
1095 if((axis >= 0) && (axis < 3))
1100 else if((axis >= 3) && (axis < 6))
1111 if((axis >= 0) && (axis < 3))
1116 else if((axis >= 3) && (axis < 6))
static btVector3 vTwist(1, 0, 0)
#define CONETWIST_USE_OBSOLETE_SOLVER
#define CONETWIST_DEF_FIX_THRESH
btScalar computeAngularImpulseDenominator(const btVector3 &axis, const btMatrix3x3 &invInertiaWorld)
@ BT_CONETWIST_FLAGS_LIN_CFM
@ BT_CONETWIST_FLAGS_LIN_ERP
@ BT_CONETWIST_FLAGS_ANG_CFM
const T & btMax(const T &a, const T &b)
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
btQuaternion shortestArcQuat(const btVector3 &v0, const btVector3 &v1)
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
btScalar btAtan2(btScalar x, btScalar y)
btScalar btSin(btScalar x)
btScalar btFabs(btScalar x)
#define SIMD_FORCE_INLINE
btScalar btCos(btScalar x)
btScalar btAtan2Fast(btScalar y, btScalar x)
bool btFuzzyZero(btScalar x)
#define btAssertConstrParams(_par)
@ CONETWIST_CONSTRAINT_TYPE
void btPlaneSpace1(const T &n, T &p, T &q)
const btRigidBody & getRigidBodyB() const
btScalar m_accSwingLimitImpulse
btScalar m_accTwistLimitImpulse
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
virtual void getInfo1(btConstraintInfo1 *info)
internal method used by the constraint solver, don't use them directly
bool m_useSolveConstraintObsolete
btVector3 m_accMotorImpulse
btScalar m_twistCorrection
btScalar m_swingLimitRatio
btScalar m_twistLimitSign
virtual void solveConstraintObsolete(btSolverBody &bodyA, btSolverBody &bodyB, btScalar timeStep)
internal method used by the constraint solver, don't use them directly
btConeTwistConstraint(btRigidBody &rbA, btRigidBody &rbB, const btTransform &rbAFrame, const btTransform &rbBFrame)
void getInfo2NonVirtual(btConstraintInfo2 *info, const btTransform &transA, const btTransform &transB, const btMatrix3x3 &invInertiaWorldA, const btMatrix3x3 &invInertiaWorldB)
void computeTwistLimitInfo(const btQuaternion &qTwist, btScalar &twistAngle, btVector3 &vTwistAxis)
void setLimit(int limitIndex, btScalar limitValue)
btScalar m_swingCorrection
void setMotorTargetInConstraintSpace(const btQuaternion &q)
const btRigidBody & getRigidBodyA() const
btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
btScalar m_twistLimitRatio
bool m_bNormalizedMotorStrength
void computeConeLimitInfo(const btQuaternion &qCone, btScalar &swingAngle, btVector3 &vSwingAxis, btScalar &swingLimit)
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 getInfo1NonVirtual(btConstraintInfo1 *info)
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
void calcAngleInfo2(const btTransform &transA, const btTransform &transB, const btMatrix3x3 &invInertiaWorldA, const btMatrix3x3 &invInertiaWorldB)
virtual void getInfo2(btConstraintInfo2 *info)
internal method used by the constraint solver, don't use them directly
void adjustSwingAxisToUseEllipseNormal(btVector3 &vSwingAxis) const
virtual void setFrames(const btTransform &frameA, const btTransform &frameB)
void updateRHS(btScalar timeStep)
btScalar m_maxMotorImpulse
btScalar m_relaxationFactor
void setMotorTarget(const btQuaternion &q)
Jacobian entry is an abstraction that allows to describe constraints it can be used in combination wi...
btScalar getDiagonal() const
btVector3 m_linearJointAxis
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
btMatrix3x3 transpose() const
Return the transpose of the matrix.
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
const btScalar & z() const
Return the z value.
const btScalar & y() const
Return the y value.
const btScalar & x() const
Return the x value.
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
btScalar getAngle() const
Return the angle [0, 2Pi] of rotation represented by this quaternion.
btQuaternion inverse() const
Return the inverse of this quaternion.
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
The btRigidBody is the main class for rigid body objects.
btScalar computeAngularImpulseDenominator(const btVector3 &axis) const
btScalar getInvMass() const
const btVector3 & getInvInertiaDiagLocal() const
const btTransform & getCenterOfMassTransform() const
const btVector3 & getCenterOfMassPosition() const
const btMatrix3x3 & getInvInertiaTensorWorld() const
TypedConstraint is the baseclass for Bullet constraints and vehicles.
btScalar m_appliedImpulse
btVector3 can be used to represent 3D points and vectors.
void setZ(btScalar _z)
Set the z value.
const btScalar & z() const
Return the z value.
btScalar length() const
Return the length of the vector.
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
btScalar dot(const btVector3 &v) const
Return the dot product.
void getSkewSymmetricMatrix(btVector3 *v0, btVector3 *v1, btVector3 *v2) const
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
btVector3 normalized() const
Return a normalized version of this vector.
void setY(btScalar _y)
Set the y value.
btScalar length2() const
Return the length of the vector squared.
const btScalar & x() const
Return the x value.
btVector3 & normalize()
Normalize this vector x^2 + y^2 + z^2 = 1.
const btScalar & y() const
Return the y value.
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
void internalApplyImpulse(const btVector3 &linearComponent, const btVector3 &angularComponent, const btScalar impulseMagnitude)
void internalGetAngularVelocity(btVector3 &angVel) const
void internalGetVelocityInLocalPointObsolete(const btVector3 &rel_pos, btVector3 &velocity) const
btScalar * m_J2angularAxis
btScalar * m_J1linearAxis
btScalar * m_J2linearAxis
btScalar * m_J1angularAxis
btScalar * m_constraintError