26 #define CONETWIST_USE_OBSOLETE_SOLVER false
27 #define CONETWIST_DEF_FIX_THRESH btScalar(.05f)
111 calcAngleInfo2(transA, transB, invInertiaWorldA, invInertiaWorldB);
124 a1neg.getSkewSymmetricMatrix(angular0, angular1, angular2);
134 a2.getSkewSymmetricMatrix(angular0, angular1, angular2);
140 for (j = 0; j < 3; j++)
151 int srow = row * info->
rowskip;
161 btVector3 p = trA.getBasis().getColumn(1);
162 btVector3 q = trA.getBasis().getColumn(2);
163 int srow1 = srow + info->
rowskip;
167 J1[srow1 + 0] = q[0];
168 J1[srow1 + 1] = q[1];
169 J1[srow1 + 2] = q[2];
170 J2[srow + 0] = -p[0];
171 J2[srow + 1] = -p[1];
172 J2[srow + 2] = -p[2];
173 J2[srow1 + 0] = -q[0];
174 J2[srow1 + 1] = -q[1];
175 J2[srow1 + 2] = -q[2];
188 J1[srow + 0] = ax1[0];
189 J1[srow + 1] = ax1[1];
190 J1[srow + 2] = ax1[2];
191 J2[srow + 0] = -ax1[0];
192 J2[srow + 1] = -ax1[1];
193 J2[srow + 2] = -ax1[2];
212 J1[srow + 0] = ax1[0];
213 J1[srow + 1] = ax1[1];
214 J1[srow + 2] = ax1[2];
215 J2[srow + 0] = -ax1[0];
216 J2[srow + 1] = -ax1[1];
217 J2[srow + 2] = -ax1[2];
259 btVector3 relPos = pivotBInW - pivotAInW;
264 normal[0] = relPos.normalized();
273 for (
int i = 0; i < 3; i++)
309 bodyA.internalGetVelocityInLocalPointObsolete(rel_pos1, vel1);
311 bodyB.internalGetVelocityInLocalPointObsolete(rel_pos2, vel2);
314 for (
int i = 0; i < 3; i++)
320 rel_vel =
normal.dot(vel);
323 btScalar impulse = depth * tau / timeStep * jacDiagABInv - rel_vel * jacDiagABInv;
340 bodyA.internalGetAngularVelocity(omegaA);
342 bodyB.internalGetAngularVelocity(omegaB);
344 trAPred.setIdentity();
347 trACur, zerovec, omegaA, timeStep, trAPred);
349 trBPred.setIdentity();
351 trBCur, zerovec, omegaB, timeStep, trBPred);
371 btScalar kAxisAInv = 0, kAxisBInv = 0;
375 axisA = dOmegaA.normalized();
381 axisB = dOmegaB.normalized();
385 btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;
387 static bool bDoTorque =
true;
393 btScalar kInvCombined = kAxisAInv + kAxisBInv;
395 btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
396 (kInvCombined * kInvCombined);
402 fMaxImpulse = fMaxImpulse / kAxisAInv;
405 btScalar newUnclampedMag = newUnclampedAccImpulse.length();
406 if (newUnclampedMag > fMaxImpulse)
408 newUnclampedAccImpulse.normalize();
409 newUnclampedAccImpulse *= fMaxImpulse;
415 btScalar impulseMag = impulse.length();
416 btVector3 impulseAxis = impulse / impulseMag;
425 bodyA.internalGetAngularVelocity(angVelA);
427 bodyB.internalGetAngularVelocity(angVelB);
431 btVector3 relVelAxis = relVel.normalized();
437 btScalar impulseMag = impulse.length();
438 btVector3 impulseAxis = impulse / impulseMag;
448 bodyA.internalGetAngularVelocity(angVelA);
450 bodyB.internalGetAngularVelocity(angVelB);
472 btVector3 impulseNoTwistCouple = impulse - impulseTwistCouple;
473 impulse = impulseNoTwistCouple;
476 impulseMag = impulse.length();
477 btVector3 noTwistSwingAxis = impulse / impulseMag;
522 btVector3 b1Axis1(0, 0, 0), b1Axis2(0, 0, 0), b1Axis3(0, 0, 0);
523 btVector3 b2Axis1(0, 0, 0), b2Axis2(0, 0, 0);
538 swx = b2Axis1.dot(b1Axis1);
539 swy = b2Axis1.dot(b1Axis2);
541 fact = (swy * swy + swx * swx) * thresh * thresh;
542 fact = fact / (fact +
btScalar(1.0));
549 swx = b2Axis1.dot(b1Axis1);
550 swy = b2Axis1.dot(b1Axis3);
552 fact = (swy * swy + swx * swx) * thresh * thresh;
553 fact = fact / (fact +
btScalar(1.0));
559 btScalar EllipseAngle =
btFabs(swing1 * swing1) * RMaxAngle1Sq +
btFabs(swing2 * swing2) * RMaxAngle2Sq;
561 if (EllipseAngle > 1.0f)
566 m_swingAxis = b2Axis1.cross(b1Axis2 * b2Axis1.dot(b1Axis2) + b1Axis3 * b2Axis1.dot(b1Axis3));
568 btScalar swingAxisSign = (b2Axis1.dot(b1Axis1) >= 0.0f) ? 1.0f : -1.0f;
619 btTransform trDeltaAB = trB * trPose * trA.inverse();
622 btScalar swingAxisLen2 = swingAxis.length2();
645 vConeNoTwist.normalize();
653 btScalar swingAngle, swingLimit = 0;
756 target[0] =
x * ivA[0] +
y * jvA[0] +
z * kvA[0];
757 target[1] =
x * ivA[1] +
y * jvA[1] +
z * kvA[1];
758 target[2] =
x * ivA[2] +
y * jvA[2] +
z * kvA[2];
815 vSwingAxis =
btVector3(qCone.x(), qCone.y(), qCone.z());
816 vSwingAxis.normalize();
830 btScalar yEllipse = -vSwingAxis.z();
842 btScalar surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse);
862 else if (swingAngle < 0)
887 btScalar surfaceSlope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse);
896 btVector3 vSwingAxis(0, xEllipse, -yEllipse);
898 btVector3 vPointInConstraintSpace(fLength, 0, 0);
899 return quatRotate(qSwing, vPointInConstraintSpace);
913 qMinTwist = -(qTwist);
924 vTwistAxis =
btVector3(qMinTwist.x(), qMinTwist.y(), qMinTwist.z());
926 vTwistAxis.normalize();
957 vSwingAxis.normalize();
998 if (swingAngle > swingLimit * softness)
999 swingAngle = swingLimit * softness;
1000 else if (swingAngle < -swingLimit * softness)
1001 swingAngle = -swingLimit * softness;
1036 if ((axis >= 0) && (axis < 3))
1048 if ((axis >= 0) && (axis < 3))
1073 if ((axis >= 0) && (axis < 3))
1078 else if ((axis >= 3) && (axis < 6))
1089 if ((axis >= 0) && (axis < 3))
1094 else if ((axis >= 3) && (axis < 6))
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble z
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint y
static btVector3 vTwist(1, 0, 0)
#define CONETWIST_USE_OBSOLETE_SOLVER
#define CONETWIST_DEF_FIX_THRESH
SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3 &axis, const btMatrix3x3 &invInertiaWorld)
virtual void getInfo2(btConstraintInfo2 *info)
virtual void buildJacobian()
internal method used by the constraint solver, don't use them directly
virtual void setParam(int num, btScalar value, int axis=-1)
virtual void setFrames(const btTransform &frameA, const btTransform &frameB)
btScalar m_twistLimitSign
btScalar m_accSwingLimitImpulse
btScalar m_accTwistLimitImpulse
void updateRHS(btScalar timeStep)
btScalar m_swingLimitRatio
bool m_bNormalizedMotorStrength
@ BT_CONETWIST_FLAGS_LIN_CFM
@ BT_CONETWIST_FLAGS_LIN_ERP
@ BT_CONETWIST_FLAGS_ANG_CFM
bool m_useSolveConstraintObsolete
btScalar m_maxMotorImpulse
virtual void solveConstraintObsolete(btSolverBody &bodyA, btSolverBody &bodyB, btScalar timeStep)
internal method used by the constraint solver, don't use them directly
virtual void getInfo1(btConstraintInfo1 *info)
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)
virtual btScalar getParam(int num, int axis=-1) const
return the local value of parameter
void computeTwistLimitInfo(const btQuaternion &qTwist, btScalar &twistAngle, btVector3 &vTwistAxis)
const btRigidBody & getRigidBodyA() const
btScalar m_twistCorrection
btScalar m_twistLimitRatio
void setMotorTargetInConstraintSpace(const btQuaternion &q)
btVector3 m_accMotorImpulse
btVector3 GetPointForAngle(btScalar fAngleInRadians, btScalar fLength) const
void computeConeLimitInfo(const btQuaternion &qCone, btScalar &swingAngle, btVector3 &vSwingAxis, btScalar &swingLimit)
btScalar m_swingCorrection
const btRigidBody & getRigidBodyB() const
void calcAngleInfo2(const btTransform &transA, const btTransform &transB, const btMatrix3x3 &invInertiaWorldA, const btMatrix3x3 &invInertiaWorldB)
void setLimit(int limitIndex, btScalar limitValue)
void adjustSwingAxisToUseEllipseNormal(btVector3 &vSwingAxis) const
btScalar m_relaxationFactor
void getInfo1NonVirtual(btConstraintInfo1 *info)
void setMotorTarget(const btQuaternion &q)
btFixedConstraint btRigidBody & rbB
btMatrix3x3
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
SIMD_FORCE_INLINE const T & btMax(const T &a, const T &b)
SIMD_FORCE_INLINE btQuaternion shortestArcQuat(const btVector3 &v0, const btVector3 &v1)
SIMD_FORCE_INLINE btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
SIMD_FORCE_INLINE btScalar btCos(btScalar x)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
SIMD_FORCE_INLINE btScalar btSin(btScalar x)
SIMD_FORCE_INLINE btScalar btFabs(btScalar x)
SIMD_FORCE_INLINE bool btFuzzyZero(btScalar x)
SIMD_FORCE_INLINE btScalar btAtan2Fast(btScalar y, btScalar x)
#define SIMD_FORCE_INLINE
SIMD_FORCE_INLINE btScalar btAtan2(btScalar x, btScalar y)
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
btSimdScalar m_appliedImpulse
#define btAssertConstrParams(_par)
btTypedConstraint(btTypedConstraintType type, btRigidBody &rbA)
@ CONETWIST_CONSTRAINT_TYPE
SIMD_FORCE_INLINE void btPlaneSpace1(const T &n, T &p, T &q)
SIMD_FORCE_INLINE btScalar norm() const
Return the norm (length) of the vector.
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
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.
SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3 &axis) const
btScalar getInvMass() const
const btTransform & getCenterOfMassTransform() const
const btVector3 & getCenterOfMassPosition() const
const btMatrix3x3 & getInvInertiaTensorWorld() const
const btVector3 & getInvInertiaDiagLocal() const
SyclQueue void void size_t num_bytes void
IconTextureDrawCall normal
ccl_device_inline float2 fabs(const float2 &a)
T dot(const vec_base< T, Size > &a, const vec_base< T, Size > &b)
BLI_INLINE float grad(int hash_val, float x, float y, float z)
btScalar * m_J2linearAxis
btScalar * m_J2angularAxis
btScalar * m_J1angularAxis
btScalar * m_constraintError
btScalar * m_J1linearAxis