/* calculates the torque and twist between two DNA segments, given their tangent vectors. Returns
* the vector with the torque, and also assigns the Tw value with the current value of the twist */
void body_twist_and_torque_nicked_kinkable (t_real *torque, const t_real *R1, const t_real *R2, t_real *Tw, void *p) {
t_real *gb = (t_real *) p;
/* calculate the binormal to t2 and t1 */
T1_x_T2 (torque, R1, R2);
/* this is the torque: no component along the twist */
dScaleVector3 (torque, *gb);
/* assign zero twist */
*Tw = 0.;
}
/* calculates the torque and twist between two DNA segments, given their tangent vectors. Returns
* the vector with the torque, and also assigns the Tw value with the current value of the twist */
void body_twist_and_torque_nicked_harmonic4 (t_real *torque, const t_real *R1, const t_real *R2, t_real *Tw, void *p) {
t_real theta;
t_real *par = (t_real *) p;
/* calculate the binormal to t2 and t1 */
T1_x_T2 (torque, R1, R2);
vector_unit (torque);
/* angle between the tangent vectors */
theta = theta_angle (R1, R2);
/* this is the torque: no component along the twist */
dScaleVector3 (torque, par [0] * theta + par [1] * theta*theta*theta);
/* assign zero twist */
*Tw = 0.;
}
void dxStepBody (dxBody *b, dReal h)
{
// cap the angular velocity
if (b->flags & dxBodyMaxAngularSpeed) {
const dReal max_ang_speed = b->max_angular_speed;
const dReal aspeed = dCalcVectorDot3( b->avel, b->avel );
if (aspeed > max_ang_speed*max_ang_speed) {
const dReal coef = max_ang_speed/dSqrt(aspeed);
dScaleVector3(b->avel, coef);
}
}
// end of angular velocity cap
// handle linear velocity
for (unsigned int j=0; j<3; j++) b->posr.pos[j] += h * b->lvel[j];
if (b->flags & dxBodyFlagFiniteRotation) {
dVector3 irv; // infitesimal rotation vector
dQuaternion q; // quaternion for finite rotation
if (b->flags & dxBodyFlagFiniteRotationAxis) {
// split the angular velocity vector into a component along the finite
// rotation axis, and a component orthogonal to it.
dVector3 frv; // finite rotation vector
dReal k = dCalcVectorDot3 (b->finite_rot_axis,b->avel);
frv[0] = b->finite_rot_axis[0] * k;
frv[1] = b->finite_rot_axis[1] * k;
frv[2] = b->finite_rot_axis[2] * k;
irv[0] = b->avel[0] - frv[0];
irv[1] = b->avel[1] - frv[1];
irv[2] = b->avel[2] - frv[2];
// make a rotation quaternion q that corresponds to frv * h.
// compare this with the full-finite-rotation case below.
h *= REAL(0.5);
dReal theta = k * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = frv[0] * s;
q[2] = frv[1] * s;
q[3] = frv[2] * s;
}
else {
// make a rotation quaternion q that corresponds to w * h
dReal wlen = dSqrt (b->avel[0]*b->avel[0] + b->avel[1]*b->avel[1] +
b->avel[2]*b->avel[2]);
h *= REAL(0.5);
dReal theta = wlen * h;
q[0] = dCos(theta);
dReal s = sinc(theta) * h;
q[1] = b->avel[0] * s;
q[2] = b->avel[1] * s;
q[3] = b->avel[2] * s;
}
// do the finite rotation
dQuaternion q2;
dQMultiply0 (q2,q,b->q);
for (unsigned int j=0; j<4; j++) b->q[j] = q2[j];
// do the infitesimal rotation if required
if (b->flags & dxBodyFlagFiniteRotationAxis) {
dReal dq[4];
dWtoDQ (irv,b->q,dq);
for (unsigned int j=0; j<4; j++) b->q[j] += h * dq[j];
}
}
else {
// the normal way - do an infitesimal rotation
dReal dq[4];
dWtoDQ (b->avel,b->q,dq);
for (unsigned int j=0; j<4; j++) b->q[j] += h * dq[j];
}
// normalize the quaternion and convert it to a rotation matrix
dNormalize4 (b->q);
dQtoR (b->q,b->posr.R);
// notify all attached geoms that this body has moved
dxWorldProcessContext *world_process_context = b->world->UnsafeGetWorldProcessingContext();
for (dxGeom *geom = b->geom; geom; geom = dGeomGetBodyNext (geom)) {
world_process_context->LockForStepbodySerialization();
dGeomMoved (geom);
world_process_context->UnlockForStepbodySerialization();
}
// notify the user
if (b->moved_callback != NULL) {
b->moved_callback(b);
}
// damping
if (b->flags & dxBodyLinearDamping) {
const dReal lin_threshold = b->dampingp.linear_threshold;
const dReal lin_speed = dCalcVectorDot3( b->lvel, b->lvel );
if ( lin_speed > lin_threshold) {
const dReal k = 1 - b->dampingp.linear_scale;
dScaleVector3(b->lvel, k);
}
}
if (b->flags & dxBodyAngularDamping) {
const dReal ang_threshold = b->dampingp.angular_threshold;
const dReal ang_speed = dCalcVectorDot3( b->avel, b->avel );
if ( ang_speed > ang_threshold) {
const dReal k = 1 - b->dampingp.angular_scale;
dScaleVector3(b->avel, k);
}
}
}
void CCharEntity::OnStepBefore()
{
if (IsCollisionEnabled() && !IsRagdollActive())
{
CEnvInfo EnvInfo;
vector3 Pos = GetTransform().pos_component();
Pos.y += Height;
float DistanceToGround;
if (EnvQueryMgr->GetEnvInfoAt(Pos, EnvInfo, Height + 0.1f, GetUniqueID()))
{
DistanceToGround = Pos.y - Height - EnvInfo.WorldHeight;
GroundMtl = EnvInfo.Material;
}
else
{
DistanceToGround = FLT_MAX;
GroundMtl = InvalidMaterial;
}
CRigidBody* pMasterBody = Composite->GetMasterBody();
bool BodyIsEnabled = IsEnabled();
vector3 AngularVel = pMasterBody->GetAngularVelocity();
vector3 LinearVel = pMasterBody->GetLinearVelocity();
vector3 DesiredLVelChange = DesiredLinearVel - LinearVel;
if (DistanceToGround <= 0.f)
{
// No torques now, angular velocity changes by impulse immediately to desired value
bool Actuated = DesiredAngularVel != AngularVel.y;
if (Actuated)
{
if (!BodyIsEnabled) SetEnabled(true);
pMasterBody->SetAngularVelocity(vector3(0.f, DesiredAngularVel, 0.f));
}
if (!DesiredLVelChange.isequal(vector3::Zero, 0.0001f))
{
if (!Actuated)
{
Actuated = true;
SetEnabled(true);
}
// Vertical movement for non-flying actors is impulse (jump).
// Since speed if already clamped to actor caps, we save vertical desired velocity as is.
// Spring force pushes us from below the ground.
//!!!!!!!!!!!!!!!
//!!!calc correct impulse for the spring!
//!!!!!!!!!!!!!!!
float VerticalDesVelChange = DesiredLVelChange.y - (50.0f * DistanceToGround);
float Mass = pMasterBody->GetMass();
//!!!remove calcs for Y, it is zero (optimization)!
dVector3 ODEForce;
dWorldImpulseToForce(Level->GetODEWorldID(), dReal(Level->GetStepSize()),
DesiredLVelChange.x * Mass, 0.f, DesiredLVelChange.z * Mass, ODEForce);
float SqForceMagnitude = (float)dCalcVectorLengthSquare3(ODEForce);
if (SqForceMagnitude > 0.f)
{
float MaxForceMagnitude = Mass * MaxHorizAccel;
float SqMaxForceMagnitude = MaxForceMagnitude * MaxForceMagnitude;
if (SqForceMagnitude > SqMaxForceMagnitude)
dScaleVector3(ODEForce, MaxForceMagnitude / n_sqrt(SqForceMagnitude));
dBodyAddForce(pMasterBody->GetODEBodyID(), ODEForce[0], ODEForce[1], ODEForce[2]);
}
if (VerticalDesVelChange != 0.f)
{
dWorldImpulseToForce(Level->GetODEWorldID(), dReal(Level->GetStepSize()),
0.f, VerticalDesVelChange * Mass, 0.f, ODEForce);
dBodyAddForce(pMasterBody->GetODEBodyID(), ODEForce[0], ODEForce[1], ODEForce[2]);
}
}
if (BodyIsEnabled && !Actuated && DistanceToGround > -0.002f)
{
const float FreezeThreshold = 0.00001f; //???use TINY?
bool AVelIsAlmostZero = n_fabs(AngularVel.y) < FreezeThreshold;
bool LVelIsAlmostZero = n_fabs(LinearVel.x) * (float)Level->GetStepSize() < FreezeThreshold &&
n_fabs(LinearVel.z) * (float)Level->GetStepSize() < FreezeThreshold;
if (AVelIsAlmostZero)
pMasterBody->SetAngularVelocity(vector3::Zero);
if (LVelIsAlmostZero)
pMasterBody->SetLinearVelocity(vector3::Zero);
if (AVelIsAlmostZero && LVelIsAlmostZero) SetEnabled(false);
}
}
//???!!!else (when falling) apply damping?!
}
// NOTE: do NOT call the parent class, we don't need any damping
}
void
dxJointSlider::getInfo2 ( dxJoint::Info2 *info )
{
// Added by OSRF
// If joint values of erp and cfm are negative, then ignore them.
// info->erp, info->cfm already have the global values from quickstep
if (this->erp >= 0)
info->erp = erp;
if (this->cfm >= 0)
{
info->cfm[0] = cfm;
info->cfm[1] = cfm;
info->cfm[2] = cfm;
info->cfm[3] = cfm;
info->cfm[4] = cfm;
}
int i, s = info->rowskip;
int s3 = 3 * s, s4 = 4 * s;
// pull out pos and R for both bodies. also get the `connection'
// vector pos2-pos1.
dReal *pos1, *pos2, *R1, *R2;
dVector3 c;
c[0] = c[1] = c[2] = 0;
pos1 = node[0].body->posr.pos;
R1 = node[0].body->posr.R;
if ( node[1].body )
{
pos2 = node[1].body->posr.pos;
R2 = node[1].body->posr.R;
for ( i = 0; i < 3; i++ )
c[i] = pos2[i] - pos1[i];
}
else
{
pos2 = 0;
R2 = 0;
}
// 3 rows to make body rotations equal
setFixedOrientation ( this, info, qrel, 0 );
// remaining two rows. we want: vel2 = vel1 + w1 x c ... but this would
// result in three equations, so we project along the planespace vectors
// so that sliding along the slider axis is disregarded. for symmetry we
// also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
dVector3 ax1; // joint axis in global coordinates (unit length)
dVector3 p, q; // plane space of ax1
dMultiply0_331 ( ax1, R1, axis1 );
dPlaneSpace ( ax1, p, q );
if ( node[1].body )
{
dVector3 tmp;
dCalcVectorCross3( tmp, c, p );
dScaleVector3( tmp, REAL( 0.5 ));
for ( i = 0; i < 3; i++ ) info->J1a[s3+i] = tmp[i];
for ( i = 0; i < 3; i++ ) info->J2a[s3+i] = tmp[i];
dCalcVectorCross3( tmp, c, q );
dScaleVector3( tmp, REAL( 0.5 ));
for ( i = 0; i < 3; i++ ) info->J1a[s4+i] = tmp[i];
for ( i = 0; i < 3; i++ ) info->J2a[s4+i] = tmp[i];
for ( i = 0; i < 3; i++ ) info->J2l[s3+i] = -p[i];
for ( i = 0; i < 3; i++ ) info->J2l[s4+i] = -q[i];
}
for ( i = 0; i < 3; i++ ) info->J1l[s3+i] = p[i];
for ( i = 0; i < 3; i++ ) info->J1l[s4+i] = q[i];
// compute last two elements of right hand side. we want to align the offset
// point (in body 2's frame) with the center of body 1.
dReal k = info->fps * info->erp;
if ( node[1].body )
{
dVector3 ofs; // offset point in global coordinates
dMultiply0_331 ( ofs, R2, offset );
for ( i = 0; i < 3; i++ ) c[i] += ofs[i];
info->c[3] = k * dCalcVectorDot3 ( p, c );
info->c[4] = k * dCalcVectorDot3 ( q, c );
}
else
{
dVector3 ofs; // offset point in global coordinates
for ( i = 0; i < 3; i++ ) ofs[i] = offset[i] - pos1[i];
info->c[3] = k * dCalcVectorDot3 ( p, ofs );
info->c[4] = k * dCalcVectorDot3 ( q, ofs );
if ( flags & dJOINT_REVERSE )
for ( i = 0; i < 3; ++i ) ax1[i] = -ax1[i];
}
// if the slider is powered, or has joint limits, add in the extra row
limot.addLimot ( this, info, 5, ax1, 0 );
}
void SimpleTrackedVehicleEnvironment::nearCallbackGrouserTerrain(dGeomID o1, dGeomID o2) {
dBodyID b1 = dGeomGetBody(o1);
dBodyID b2 = dGeomGetBody(o2);
if(b1 && b2 && dAreConnectedExcluding(b1, b2, dJointTypeContact)) return;
// body of the whole vehicle
dBodyID vehicleBody = ((SimpleTrackedVehicle*)this->v)->vehicleBody;
unsigned long geom1Categories = dGeomGetCategoryBits(o1);
// speeds of the belts
const dReal leftBeltSpeed = ((SimpleTrackedVehicle*)this->v)->leftTrack->getVelocity();
const dReal rightBeltSpeed = ((SimpleTrackedVehicle*)this->v)->rightTrack->getVelocity();
dReal beltSpeed = 0; // speed of the belt which is in collision and examined right now
if (geom1Categories & Category::LEFT) {
beltSpeed = leftBeltSpeed;
} else {
beltSpeed = rightBeltSpeed;
}
// the desired linear and angular speeds (set by desired track velocities)
const dReal linearSpeed = (leftBeltSpeed + rightBeltSpeed) / 2;
const dReal angularSpeed = (leftBeltSpeed - rightBeltSpeed) * steeringEfficiency / tracksDistance;
// radius of the turn the robot is doing
const dReal desiredRotationRadiusSigned = (fabs(angularSpeed) < 0.1) ?
dInfinity : // is driving straight
((fabs(linearSpeed) < 0.1) ?
0 : // is rotating about a single point
linearSpeed / angularSpeed // general movement
);
dVector3 yAxisGlobal; // vector pointing from vehicle body center in the direction of +y axis
dVector3 centerOfRotation; // at infinity if driving straight, so we need to distinguish the case
{ // compute the center of rotation
dBodyVectorToWorld(vehicleBody, 0, 1, 0, yAxisGlobal);
dCopyVector3(centerOfRotation, yAxisGlobal);
// make the unit vector as long as we need (and change orientation if needed; the radius is a signed number)
dScaleVector3(centerOfRotation, desiredRotationRadiusSigned);
const dReal *vehicleBodyPos = dBodyGetPosition(vehicleBody);
dAddVectors3(centerOfRotation, centerOfRotation, vehicleBodyPos);
}
int maxContacts = 20;
dContact contact[maxContacts];
int numContacts = dCollide(o1, o2, maxContacts, &contact[0].geom, sizeof(dContact));
for(size_t i = 0; i < numContacts; i++) {
dVector3 contactInVehiclePos; // position of the contact point relative to vehicle body
dBodyGetPosRelPoint(vehicleBody, contact[i].geom.pos[0], contact[i].geom.pos[1], contact[i].geom.pos[2], contactInVehiclePos);
dVector3 beltDirection; // vector tangent to the belt pointing in the belt's movement direction
dCalcVectorCross3(beltDirection, contact[i].geom.normal, yAxisGlobal);
if (beltSpeed > 0) {
dNegateVector3(beltDirection);
}
if (desiredRotationRadiusSigned != dInfinity) { // non-straight drive
dVector3 COR2Contact; // vector pointing from the center of rotation to the contact point
dSubtractVectors3(COR2Contact, contact[i].geom.pos, centerOfRotation);
// the friction force should be perpendicular to COR2Contact
dCalcVectorCross3(contact[i].fdir1, contact[i].geom.normal, COR2Contact);
const dReal linearSpeedSignum = (fabs(linearSpeed) > 0.1) ? sgn(linearSpeed) : 1;
// contactInVehiclePos[0] > 0 means the contact is in the front part of the track
if (sgn(angularSpeed) * sgn(dCalcVectorDot3(yAxisGlobal, contact[i].fdir1)) !=
sgn(contactInVehiclePos[0]) * linearSpeedSignum) {
dNegateVector3(contact[i].fdir1);
}
} else { // straight drive
dCalcVectorCross3(contact[i].fdir1, contact[i].geom.normal, yAxisGlobal);
if (dCalcVectorDot3(contact[i].fdir1, beltDirection) < 0) {
dNegateVector3(contact[i].fdir1);
}
}
// use friction direction and motion1 to simulate the track movement
contact[i].surface.mode = dContactFDir1 | dContactMotion1 | dContactMu2;
contact[i].surface.mu = 0.5;
contact[i].surface.mu2 = 10;
// the dot product <beltDirection,fdir1> is the cosine of the angle they form (because both are unit vectors)
contact[i].surface.motion1 = -dCalcVectorDot3(beltDirection, contact[i].fdir1) * fabs(beltSpeed) * 0.07;
// friction force visualization
dMatrix3 forceRotation;
dVector3 vec;
dBodyVectorToWorld(vehicleBody, 1, 0, 0, vec);
dRFrom2Axes(forceRotation, contact[i].fdir1[0], contact[i].fdir1[1], contact[i].fdir1[2], vec[0], vec[1], vec[2]);
posr data;
dCopyVector3(data.pos, contact[i].geom.pos);
dCopyMatrix4x3(data.R, forceRotation);
forces.push_back(data);
dJointID c = dJointCreateContact(this->world, this->contactGroup, &contact[i]);
dJointAttach(c, b1, b2);
if(!isValidCollision(o1, o2, contact[i]))
this->badCollision = true;
if(config.contact_grouser_terrain.debug)
this->contacts.push_back(contact[i].geom);
}
}
btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Cooper(btScalar step) const
{
#if 0
dReal h = callContext->m_stepperCallContext->m_stepSize; // Step size
dVector3 L; // Compute angular momentum
dMultiply0_331(L, I, b->avel);
#endif
btVector3 inertiaLocal = getLocalInertia();
btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
btVector3 L = inertiaTensorWorld*getAngularVelocity();
btMatrix3x3 Itild(0, 0, 0, 0, 0, 0, 0, 0, 0);
#if 0
for (int ii = 0; ii<12; ++ii) {
Itild[ii] = Itild[ii] * h + I[ii];
}
#endif
btSetCrossMatrixMinus(Itild, L*step);
Itild += inertiaTensorWorld;
#if 0
// Compute a new effective 'inertia tensor'
// for the implicit step: the cross-product
// matrix of the angular momentum plus the
// old tensor scaled by the timestep.
// Itild may not be symmetric pos-definite,
// but we can still use it to compute implicit
// gyroscopic torques.
dMatrix3 Itild = { 0 };
dSetCrossMatrixMinus(Itild, L, 4);
for (int ii = 0; ii<12; ++ii) {
Itild[ii] = Itild[ii] * h + I[ii];
}
#endif
L *= step;
//Itild may not be symmetric pos-definite
btMatrix3x3 itInv = Itild.inverse();
Itild = inertiaTensorWorld * itInv;
btMatrix3x3 ident(1,0,0,0,1,0,0,0,1);
Itild -= ident;
#if 0
// Scale momentum by inverse time to get
// a sort of "torque"
dScaleVector3(L, dRecip(h));
// Invert the pseudo-tensor
dMatrix3 itInv;
// This is a closed-form inversion.
// It's probably not numerically stable
// when dealing with small masses with
// a large asymmetry.
// An LU decomposition might be better.
if (dInvertMatrix3(itInv, Itild) != 0) {
// "Divide" the original tensor
// by the pseudo-tensor (on the right)
dMultiply0_333(Itild, I, itInv);
// Subtract an identity matrix
Itild[0] -= 1; Itild[5] -= 1; Itild[10] -= 1;
// This new inertia matrix rotates the
// momentum to get a new set of torques
// that will work correctly when applied
// to the old inertia matrix as explicit
// torques with a semi-implicit update
// step.
dVector3 tau0;
dMultiply0_331(tau0, Itild, L);
// Add the gyro torques to the torque
// accumulator
for (int ii = 0; ii<3; ++ii) {
b->tacc[ii] += tau0[ii];
}
#endif
btVector3 tau0 = Itild * L;
// printf("tau0 = %f,%f,%f\n",tau0.x(),tau0.y(),tau0.z());
return tau0;
}
btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Ewert(btScalar step) const
{
// use full newton-euler equations. common practice to drop the wxIw term. want it for better tumbling behavior.
// calculate using implicit euler step so it's stable.
const btVector3 inertiaLocal = getLocalInertia();
const btVector3 w0 = getAngularVelocity();
btMatrix3x3 I;
I = m_worldTransform.getBasis().scaled(inertiaLocal) *
m_worldTransform.getBasis().transpose();
// use newtons method to find implicit solution for new angular velocity (w')
// f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0
// df/dw' = I + 1xIw'*step + w'xI*step
btVector3 w1 = w0;
// one step of newton's method
{
const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I);
const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I);
const btMatrix3x3 dfw_inv = dfw.inverse();
btVector3 dw;
dw = dfw_inv*fw;
w1 -= dw;
}
btVector3 gf = (w1 - w0);
return gf;
}
void btRigidBody::integrateVelocities(btScalar step)
{
if (isStaticOrKinematicObject())
return;
m_linearVelocity += m_totalForce * (m_inverseMass * step);
m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
#define MAX_ANGVEL SIMD_HALF_PI
/// clamp angular velocity. collision calculations will fail on higher angular velocities
btScalar angvel = m_angularVelocity.length();
if (angvel*step > MAX_ANGVEL)
{
m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
}
}
btQuaternion btRigidBody::getOrientation() const
{
btQuaternion orn;
m_worldTransform.getBasis().getRotation(orn);
return orn;
}
