/* 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; }