void _dSolveCholesky (const dReal *L, dReal *b, int n, void *tmpbuf/*[n]*/)
{
dAASSERT (n > 0 && L && b);
const int nskip = dPAD (n);
dReal *y = tmpbuf ? (dReal *)tmpbuf : (dReal*) ALLOCA (n*sizeof(dReal));
{
const dReal *ll = L;
for (int i=0; i<n; ll+=nskip, ++i) {
dReal sum = REAL(0.0);
for (int k=0; k < i; ++k) {
sum += ll[k]*y[k];
}
dIASSERT(ll[i] != dReal(0.0));
y[i] = (b[i]-sum)/ll[i];
}
}
{
const dReal *ll = L + (n - 1) * (nskip + 1);
for (int i=n-1; i>=0; ll-=nskip+1, --i) {
dReal sum = REAL(0.0);
const dReal *l = ll + nskip;
for (int k=i+1; k<n; l+=nskip, ++k) {
sum += (*l)*b[k];
}
dIASSERT(*ll != dReal(0.0));
b[i] = (y[i]-sum)/(*ll);
}
}
}
void BallJoint::act(bool initial)
{
if(!initial)
{
for(unsigned int i=0; i < this->motors.size(); i++)
{
int numAxes = this->motors[i]->getNumberOfParsedAxes();
if(numAxes > 0)
{
if(this->motors[i]->getMotorType() == VELOCITY)
{
double desiredVelocity = this->motors[i]->getDesiredVelocity();
dJointSetAMotorParam(*(this->motors[i]->getPhysicalMotor()), dParamVel, dReal(desiredVelocity));
}
if(numAxes > 1)
{
if(this->motors[i]->getMotorType() == VELOCITY)
{
double desiredVelocity = this->motors[i]->getDesiredVelocity();
dJointSetAMotorParam(*(this->motors[i]->getPhysicalMotor()), dParamVel2, dReal(desiredVelocity));
}
if(numAxes > 2)
{
if(this->motors[i]->getMotorType() == VELOCITY)
{
double desiredVelocity = this->motors[i]->getDesiredVelocity();
dJointSetAMotorParam(*(this->motors[i]->getPhysicalMotor()), dParamVel3, dReal(desiredVelocity));
}
}
}
}
}
}
}
dReal randn_trig(dReal mu, dReal sigma) {
static bool deviateAvailable=false; // flag
static dReal storedDeviate; // deviate from previous calculation
dReal dist, angle;
// If no deviate has been stored, the standard Box-Muller transformation is
// performed, producing two independent normally-distributed random
// deviates. One is stored for the next round, and one is returned.
if (!deviateAvailable) {
// choose a pair of uniformly distributed deviates, one for the
// distance and one for the angle, and perform transformations
dist=sqrt( -2.0 * log(dReal(rand()) / dReal(RAND_MAX)) );
angle=2.0 * PI * (dReal(rand()) / dReal(RAND_MAX));
// calculate and store first deviate and set flag
storedDeviate=dist*cos(angle);
deviateAvailable=true;
// calcaulate return second deviate
return dist * sin(angle) * sigma + mu;
}
// If a deviate is available from a previous call to this function, it is
// returned, and the flag is set to false.
else {
deviateAvailable=false;
return storedDeviate*sigma + mu;
}
}
void Cylinder::computeMassProperties(dMass& m)
{
if(capped)
dMassSetCapsule(&m, 1, 3, dReal(height), dReal(height));
else
dMassSetCylinder(&m, 1, 3, dReal(height), dReal(height));
dMassAdjust(&m, dReal(mass));
}
void UniversalJoint::setValue(double value, int port)
{
/* TEMPORARY FIX */
if(port == AXIS1_MOTOR)
{
double newValue = value;
if(axis1->motor->getMotorType() == ANGULAR)
{
if(value == axis1->maxValue || value == axis1->minValue)
{
if(value == 0.0 && axis1->minValue == 0.0)
{
newValue = -0.01;
}
else if(value == 0.0 && axis1->maxValue == 0.0)
{
newValue = 0.01;
}
else
{
newValue -= (value *dReal(0.01));
}
}
newValue = Functions::toRad(newValue);
}
axis1->motor->setValue(newValue, port);
}
else if(port == AXIS2_MOTOR)
{
double newValue = value;
if(axis2->motor->getMotorType() == ANGULAR)
{
if(value == axis2->maxValue || value == axis2->minValue)
{
if(value == 0.0 && axis2->minValue == 0.0)
{
newValue = -0.01;
}
else if(value == 0.0 && axis2->maxValue == 0.0)
{
newValue = 0.01;
}
else
{
newValue -= (value *dReal(0.01));
}
}
newValue = Functions::toRad(newValue);
}
axis2->motor->setValue(newValue, port);
}
}
void BallJoint::createJointPhysics()
{
this->physicalJoint = dJointCreateBall(*this->simulation->getPhysicalWorld(), 0);
PhysicalObject* o1 = this->getPhysicalParentObject();
PhysicalObject* o2 = this->getPhysicalChildObject(this);
assert (o1 != NULL && o2 != NULL);
if(o1 && o2)
{
dJointAttach(this->physicalJoint, *(o2->getBody()), *(o1->getBody()));
//set ball joint parameter
dJointSetBallAnchor(this->physicalJoint, dReal(anchorPoint.v[0]), dReal(anchorPoint.v[1]), dReal(anchorPoint.v[2]));
}
}
void CappedCylinder::createGeometry(dSpaceID space)
{
if(geometry == 0)
{
setGeometry(dCreateCCylinder(space, dReal(radius), dReal(height)));
dGeomSetPosition(geometry, dReal(position.v[0]), dReal(position.v[1]), dReal(position.v[2]));
dMatrix3 rotationMatrix;
ODETools::convertMatrix(rotation, rotationMatrix);
dGeomSetRotation(geometry, rotationMatrix);
//set user data pointer of ODE geometry object to this physical object
dGeomSetData(geometry, this);
//set collide bitfield
PhysicalObject::setCollideBitfield();
}
}
void wrl::atom::get_surface(dSurfaceParameters& s) const
{
// Merge this atom's surface parameters with the given structure.
param_map::const_iterator i;
if ((i = params.find(wrl::param::mu)) != params.end())
s.mu = std::min(s.mu, dReal(i->second->value()));
if ((i = params.find(wrl::param::bounce)) != params.end())
s.bounce = std::max(s.bounce, dReal(i->second->value()));
if ((i = params.find(wrl::param::soft_erp)) != params.end())
s.soft_erp = std::min(s.soft_erp, dReal(i->second->value()));
if ((i = params.find(wrl::param::soft_cfm)) != params.end())
s.soft_cfm = std::max(s.soft_cfm, dReal(i->second->value()));
}
void UniversalJoint::applyFriction(bool initial)
{
if(!initial)
{
double velocity = 0.0;
double gravityFactor = fabs((simulation->getPhysicsParameters())->getGravity());
double fmax;
if(axis1->motor == 0)
{
fmax = axis1->friction * gravityFactor;
dJointSetUniversalParam(physicalJoint, dParamFMax, dReal(fmax));
dJointSetUniversalParam(physicalJoint, dParamVel, dReal(velocity));
}
if(axis2->motor == 0)
{
fmax = axis2->friction * gravityFactor;
dJointSetUniversalParam(physicalJoint, dParamFMax2, dReal(fmax));
dJointSetUniversalParam(physicalJoint, dParamVel2, dReal(velocity));
}
}
}
void _dLDLTRemove (dReal **A, const int *p, dReal *L, dReal *d,
int n1, int n2, int r, int nskip, void *tmpbuf/*n2 + 2*nskip*/)
{
dAASSERT(A && p && L && d && n1 > 0 && n2 > 0 && r >= 0 && r < n2 &&
n1 >= n2 && nskip >= n1);
#ifndef dNODEBUG
for (int i=0; i<n2; ++i) dIASSERT(p[i] >= 0 && p[i] < n1);
#endif
if (r==n2-1) {
return; // deleting last row/col is easy
}
else {
size_t LDLTAddTL_size = _dEstimateLDLTAddTLTmpbufSize(nskip);
dIASSERT(LDLTAddTL_size % sizeof(dReal) == 0);
dReal *tmp = tmpbuf ? (dReal *)tmpbuf : (dReal*) ALLOCA (LDLTAddTL_size + n2 * sizeof(dReal));
if (r==0) {
dReal *a = (dReal *)((char *)tmp + LDLTAddTL_size);
const int p_0 = p[0];
for (int i=0; i<n2; ++i) {
a[i] = -GETA(p[i],p_0);
}
a[0] += REAL(1.0);
dLDLTAddTL (L,d,a,n2,nskip,tmp);
}
else {
dReal *t = (dReal *)((char *)tmp + LDLTAddTL_size);
{
dReal *Lcurr = L + r*nskip;
for (int i=0; i<r; ++Lcurr, ++i) {
dIASSERT(d[i] != dReal(0.0));
t[i] = *Lcurr / d[i];
}
}
dReal *a = t + r;
{
dReal *Lcurr = L + r*nskip;
const int *pp_r = p + r, p_r = *pp_r;
const int n2_minus_r = n2-r;
for (int i=0; i<n2_minus_r; Lcurr+=nskip,++i) {
a[i] = dDot(Lcurr,t,r) - GETA(pp_r[i],p_r);
}
}
a[0] += REAL(1.0);
dLDLTAddTL (L + r*nskip+r, d+r, a, n2-r, nskip, tmp);
}
}
// snip out row/column r from L and d
dRemoveRowCol (L,n2,nskip,r);
if (r < (n2-1)) memmove (d+r,d+r+1,(n2-r-1)*sizeof(dReal));
}
void ServoMotor::act()
{
const float currentPos = dJointGetType(joint->joint) == dJointTypeHinge
? dJointGetHingeAngle(joint->joint)
: dJointGetSliderPosition(joint->joint);
float setpoint = this->setpoint;
const float maxValueChange = maxVelocity * Simulation::simulation->scene->stepLength;
if(std::abs(setpoint - currentPos) > maxValueChange)
{
if(setpoint < currentPos)
setpoint = currentPos - maxValueChange;
else
setpoint = currentPos + maxValueChange;
}
const float newVel = controller.getOutput(currentPos, setpoint);
if(dJointGetType(joint->joint) == dJointTypeHinge)
dJointSetHingeParam(joint->joint, dParamVel, dReal(newVel));
else
dJointSetSliderParam(joint->joint, dParamVel, dReal(newVel));
}
void Hinge::createPhysics()
{
ASSERT(axis);
ASSERT(axis->motor);
//
axis->create();
//
PrimaryObject::createPhysics();
// find bodies to connect
Body* parentBody = dynamic_cast<Body*>(parent);
ASSERT(!parentBody || parentBody->body);
ASSERT(!children.empty());
Body* childBody = dynamic_cast<Body*>(*children.begin());
ASSERT(childBody);
ASSERT(childBody->body);
// create joint
joint = dJointCreateHinge(Simulation::simulation->physicalWorld, 0);
dJointAttach(joint, childBody->body, parentBody ? parentBody->body : 0);
//set hinge joint parameter
dJointSetHingeAnchor(joint, pose.translation.x, pose.translation.y, pose.translation.z);
Vector3<> globalAxis = pose.rotation * Vector3<>(axis->x, axis->y, axis->z);
dJointSetHingeAxis(joint, globalAxis.x, globalAxis.y, globalAxis.z);
if(axis->cfm != -1.f)
dJointSetHingeParam(joint, dParamCFM, dReal(axis->cfm));
if(axis->deflection)
{
const Axis::Deflection& deflection = *axis->deflection;
float minHingeLimit = deflection.min;
float maxHingeLimit = deflection.max;
if(minHingeLimit > maxHingeLimit)
minHingeLimit = maxHingeLimit;
//Set physical limits to higher values (+10%) to avoid strange hinge effects.
//Otherwise, sometimes the motor exceeds the limits.
float internalTolerance = (maxHingeLimit - minHingeLimit) * 0.1f;
if(dynamic_cast<ServoMotor*>(axis->motor))
{
minHingeLimit -= internalTolerance;
maxHingeLimit += internalTolerance;
}
dJointSetHingeParam(joint, dParamLoStop, dReal(minHingeLimit));
dJointSetHingeParam(joint, dParamHiStop, dReal(maxHingeLimit));
// this has to be done due to the way ODE sets joint stops
dJointSetHingeParam(joint, dParamLoStop, dReal(minHingeLimit));
if(deflection.stopCFM != -1.f)
dJointSetHingeParam(joint, dParamStopCFM, dReal(deflection.stopCFM));
if(deflection.stopERP != -1.f)
dJointSetHingeParam(joint, dParamStopERP, dReal(deflection.stopERP));
}
// create motor
axis->motor->create(this);
OpenGLTools::convertTransformation(rotation, translation, transformation);
}
void BallJoint::createMotorPhysics()
{
for(unsigned int i=0; i<this->motors.size(); i++)
{
dJointID physMotor = dJointCreateAMotor(*(this->simulation->getPhysicalWorld()), 0);
this->motors[i]->setPhysicalMotor(physMotor);
dJointAttach(physMotor, dJointGetBody(this->physicalJoint, 0), dJointGetBody(this->physicalJoint, 1));
if(this->motors[i]->getKind() == "eulermotor")
dJointSetAMotorMode(physMotor, dAMotorEuler);
else
dJointSetAMotorMode(physMotor, dAMotorUser);
//user defined motor
if(this->motors[i]->getKind() == "userdefinedmotor")
{
unsigned short numOfAxes = this->motors[i]->getNumberOfParsedAxes();
dJointSetAMotorNumAxes(physMotor, numOfAxes);
dBodyID b1= dJointGetBody(this->physicalJoint, 0);
if(b1 != NULL)
{
for(int j=0; j < numOfAxes; j++)
{
MotorAxisDescription* ax = this->motors[i]->getAxis(j);
dJointSetAMotorAxis(physMotor, j, 1, dReal(ax->direction.v[0]), dReal(ax->direction.v[1]), dReal(ax->direction.v[2]));
}
}
else
{
for(int j=0; j < numOfAxes; j++)
{
MotorAxisDescription* ax = this->motors[i]->getAxis(j);
dJointSetAMotorAxis(physMotor, j, 0, dReal(ax->direction.v[0]), dReal(ax->direction.v[1]), dReal(ax->direction.v[2]));
}
}
//maxForce and maxVelocity are stored in each axis of the motor
dJointSetAMotorParam(physMotor, dParamFMax, dReal(this->motors[i]->getAxis(0)->maxForce));
if(numOfAxes > 1)
{
dJointSetAMotorParam(physMotor, dParamFMax2, dReal(this->motors[i]->getAxis(1)->maxForce));
if(numOfAxes > 2)
{
dJointSetAMotorParam(physMotor, dParamFMax3, dReal(this->motors[i]->getAxis(2)->maxForce));
}
}
//stops are useless for balljoint
}
}
}
// "Polar" version without trigonometric calls
dReal randn_notrig(dReal mu, dReal sigma) {
if (sigma==0) return mu;
static bool deviateAvailable=false; // flag
static dReal storedDeviate; // deviate from previous calculation
dReal polar, rsquared, var1, var2;
// If no deviate has been stored, the polar Box-Muller transformation is
// performed, producing two independent normally-distributed random
// deviates. One is stored for the next round, and one is returned.
if (!deviateAvailable) {
// choose pairs of uniformly distributed deviates, discarding those
// that don't fall within the unit circle
do {
var1=2.0*( dReal(rand())/dReal(RAND_MAX) ) - 1.0;
var2=2.0*( dReal(rand())/dReal(RAND_MAX) ) - 1.0;
rsquared=var1*var1+var2*var2;
} while ( rsquared>=1.0 || rsquared == 0.0);
// calculate polar tranformation for each deviate
polar=sqrt(-2.0*log(rsquared)/rsquared);
// store first deviate and set flag
storedDeviate=var1*polar;
deviateAvailable=true;
// return second deviate
return var2*polar*sigma + mu;
}
// If a deviate is available from a previous call to this function, it is
// returned, and the flag is set to false.
else {
deviateAvailable=false;
return storedDeviate*sigma + mu;
}
}
void simLoop(int pause)
{
if (!pause) {
dSpaceCollide (space, 0, &nearCallback);
dWorldQuickStep(world, 0.01);
dJointGroupEmpty(contactgroup);
}
dsSetColor (1,1,0);
for (int i=0; i<ncards; ++i) {
dsSetColor (1, dReal(i)/ncards, 0);
cards[i]->draw();
}
}
void Cylinder::createBody()
{
if(body == 0)
{
body = dBodyCreate(*simulation->getPhysicalWorld());
dMass m;
if(capped)
dMassSetCapsule(&m, 1, 3, dReal(height), dReal(height));
else
dMassSetCylinder(&m, 1, 3, dReal(height), dReal(height));
dMassAdjust(&m, dReal(mass));
dBodySetMass(body, &m);
dBodySetPosition(body, dReal(position.v[0]), dReal(position.v[1]), dReal(position.v[2]));
dMatrix3 rotationMatrix;
ODETools::convertMatrix(rotation, rotationMatrix);
dBodySetRotation(body, rotationMatrix);
}
}
//-------------------------------------------------------------------------
void World::_applyDynamics(Real timeElapsed)
{
if (timeElapsed != 0.0f)
{
// ODE will throw an error if timestep = 0
mOdeWorld->step(dReal(timeElapsed));
// Now update the objects in the world
ObjectSet::iterator i, iend;
iend = mDynamicsObjects.end();
for (i = mDynamicsObjects.begin(); i != iend; ++i)
{
(*i)->_updateFromDynamics();
}
// Clear contacts
mOdeContactGroup->empty();
}
}
EXPORT_C void dQFromAxisAndAngle (dQuaternion q, dReal ax, dReal ay, dReal az,
dReal angle)
{
dReal l = dMUL(ax,ax) + dMUL(ay,ay) + dMUL(az,az);
if (l > REAL(0.0)) {
angle = dMUL(angle,REAL(0.5));
q[0] = dCos (angle);
l = dMUL(dReal(dSin(angle)),dRecipSqrt(l));
q[1] = dMUL(ax,l);
q[2] = dMUL(ay,l);
q[3] = dMUL(az,l);
}
else {
q[0] = REAL(1.0);
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
}
void UniversalJoint::act(bool initial)
{
if(!initial)
{
if(axis1->motor)
{
if(axis1->motor->getMotorType() == VELOCITY)
{
dReal desiredVelocity(dReal(axis1->motor->getDesiredVelocity()));
dJointSetUniversalParam(physicalJoint, dParamFMax, dReal(axis1->motor->getMaxForce()));
dJointSetUniversalParam(physicalJoint, dParamVel, desiredVelocity);
}
else if(axis1->motor->getMotorType() == ANGULAR)
{
double controllerOutput = axis1->motor->getControllerOutput(dJointGetUniversalAngle1(physicalJoint));
dJointSetUniversalParam(physicalJoint, dParamFMax, dReal(axis1->motor->getMaxForce()));
dJointSetUniversalParam(physicalJoint, dParamVel, dReal(controllerOutput));
}
}
if(axis2->motor)
{
if(axis2->motor->getMotorType() == VELOCITY)
{
dReal desiredVelocity(dReal(axis2->motor->getDesiredVelocity()));
dJointSetUniversalParam(physicalJoint, dParamFMax2, dReal(axis2->motor->getMaxForce()));
dJointSetUniversalParam(physicalJoint, dParamVel2, desiredVelocity);
}
else if(axis2->motor->getMotorType() == ANGULAR)
{
double controllerOutput = axis2->motor->getControllerOutput(dJointGetUniversalAngle2(physicalJoint));
dJointSetUniversalParam(physicalJoint, dParamFMax2, dReal(axis2->motor->getMaxForce()));
dJointSetUniversalParam(physicalJoint, dParamVel2, dReal(controllerOutput));
}
}
}
}
//Trigger the ODE simulation. This method should be called frequently
//(call SetTime() before invoking Trigger() to update the current Time).
//The method will invoke dWorldStep one or several times, depending
//on the Time since the last call, and the step size of the Level.
//The method will make sure that the physics simulation is triggered
//using a constant step size.
void CLevel::Trigger()
{
PROFILER_START(profFrameBefore);
for (int i = 0; i < Entities.Size(); i++) Entities[i]->OnFrameBefore();
PROFILER_STOP(profFrameBefore);
PROFILER_RESET(profStepBefore);
PROFILER_RESET(profStepAfter);
PROFILER_RESET(profCollide);
PROFILER_RESET(profStep);
PROFILER_RESET(profJointGroupEmpty);
#ifdef __NEBULA_STATS__
statsNumNearCallbackCalled = 0;
statsNumCollideCalled = 0;
statsNumCollided = 0;
statsNumSpaceCollideCalled = 0;
statsNumSteps = 0;
#endif
TimeToSim += GameSrv->GetFrameTime();
while (TimeToSim > StepSize)
{
PROFILER_STARTACCUM(profStepBefore);
for (int i = 0; i < Entities.Size(); i++) Entities[i]->OnStepBefore();
PROFILER_STOPACCUM(profStepBefore);
// do collision detection
PROFILER_STARTACCUM(profCollide);
statsNumSpaceCollideCalled++;
dSpaceCollide2((dGeomID)ODEDynamicSpaceID, (dGeomID)ODEStaticSpaceID, this, &ODENearCallback);
dSpaceCollide(ODEDynamicSpaceID, this, &ODENearCallback);
PROFILER_STOPACCUM(profCollide);
// step physics simulation
PROFILER_STARTACCUM(profStep);
dWorldQuickStep(ODEWorldID, dReal(StepSize));
PROFILER_STOPACCUM(profStep);
// clear Contact joints
PROFILER_STARTACCUM(profJointGroupEmpty);
dJointGroupEmpty(ContactJointGroup);
PROFILER_STOPACCUM(profJointGroupEmpty);
PROFILER_STARTACCUM(profStepAfter);
for (int i = 0; i < Entities.Size(); i++) Entities[i]->OnStepAfter();
PROFILER_STOPACCUM(profStepAfter);
statsNumSteps++;
TimeToSim -= StepSize;
}
// export statistics
#ifdef __NEBULA_STATS__
//nWatched watchSpaceCollideCalled("statsMangaPhysicsSpaceCollideCalled", DATA_TYPE(int));
//nWatched watchNearCallbackCalled("statsMangaPhysicsNearCallbackCalled", DATA_TYPE(int));
//nWatched watchCollideCalled("statsMangaPhysicsCollideCalled", DATA_TYPE(int));
//nWatched watchCollided("statsMangaPhysicsCollided", DATA_TYPE(int));
//nWatched watchSpaces("statsMangaPhysicsSpaces", DATA_TYPE(int));
//nWatched watchShapes("statsMangaPhysicsShapes", DATA_TYPE(int));
//nWatched watchSteps("statsMangaPhysicsSteps", DATA_TYPE(int));
//if (statsNumSteps > 0)
//{
// watchSpaceCollideCalled->SetValue(statsNumSpaceCollideCalled/statsNumSteps);
// watchNearCallbackCalled->SetValue(statsNumNearCallbackCalled/statsNumSteps);
// watchCollideCalled->SetValue(statsNumCollideCalled/statsNumSteps);
// watchCollided->SetValue(statsNumCollided/statsNumSteps);
//}
//watchSpaces->SetValue(statsNumSpaces);
//watchShapes->SetValue(statsNumShapes);
//watchSteps->SetValue(statsNumSteps);
#endif
// invoke the "on-frame-after" methods
PROFILER_START(profFrameAfter);
for (int i = 0; i < Entities.Size(); i++) Entities[i]->OnFrameAfter();
PROFILER_STOP(profFrameAfter);
}
/*** P control tries to add a force that reduces the distance between
the current joint angles and the desired value in the lookup table THETA.
***/
void Pcontrol()
{
dReal kp = 2.0; //effects how quickly it tries to acheive the desired angle (original=2)
// dReal kp = 10.0; //effects how quickly it tries to acheive the desired angle
dReal fMax = 100.0; //fMax is the max for it can apply trying to acheive the desired angle
//#define ANG_VELOCITY
#ifndef ANG_VELOCITY
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
dReal tmp = dJointGetHingeAngle(leg[segment][i][j].joint);
dReal diff = THETA[segment][i][j] - tmp;
dReal u;
u = kp * diff;
//cout << "\n\n***" << u << "***\n\n\n\n" << endl;
dJointSetHingeParam(leg[segment][i][j].joint, dParamVel,
u);
dJointSetHingeParam(leg[segment][i][j].joint, dParamFMax,
fMax);
}
}
#else //when using angular velocity
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
//dReal tmp = dJointGetHingeAngle(leg[segment][i][j].joint);
//dReal diff = THETA[segment][i][j] - tmp;
//dReal u;
//u = kp * diff;
dReal desiredValue = THETA[segment][i][j];
desiredValue *= 2.25;
//cout << "\n\n***" << desiredValue << "***\n\n\n\n" << endl;
dJointSetHingeParam(leg[segment][i][j].joint, dParamVel,
desiredValue);
dJointSetHingeParam(leg[segment][i][j].joint, dParamFMax,
fMax);
}
}
#endif
#ifdef USE_NLEG_ROBOT
if(segment < BODY_SEGMENTS-1) {
// applying torso joint force
dReal tmp = dJointGetHingeAngle(torso[segment].joint); //joints might start in segment 2
dReal diff = torsoJointDesiredAngle[segment] - tmp;
dReal u;
u = kp * diff;
dJointSetHingeParam(torso[segment].joint, dParamVel, u);
dJointSetHingeParam(torso[segment].joint, dParamFMax, fMax);
}
#endif
}
}
/*** Control of walking ***/
void walk()
{
// int numSheets;
//
// #ifdef HIDDEN_LAYER
// numSheets = 3;
// #else
// numSheets = 2;
// #endif
timeSteps++;
//jmc added to print out joint angles
if(printJointAngles){
static bool doOnce = true;
if(doOnce){
doOnce=false;
//remove the file if it is there already
ofstream jangle_file;
jangle_file.open ("jointAngles.txt", ios::trunc );
jangle_file.close();
}
}
//clear out the network
//LHZ - When Recurrence is turned on, we multiply the input values by the recurrent values that were in
//the input nodes to begin with, if they are all 0, the network will never take a value. So we start it with
//all 1's, since this will act as if ther was no recurrence for the first update.
substrate_pointer->reinitialize();
if(experimentType != 33) //If it is a NEAT experiment, the substrate can have an arbitrary number of layers, so we need to update 1+ExtraActivationUpdates num times
{
substrate_pointer->dummyActivation(); //dummyActivation sets a flag so that the first update does not add on ExtraActivationUdates num updates
}
dReal roll, pitch, yaw;
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
roll = pitch = yaw = 0.0;
const dReal * torsoRotation = dBodyGetRotation(torso[segment].body);
get_euler(torsoRotation, roll, pitch, yaw);
#if LEGSWING_ODE_DEBUG
printf("roll: %f, pitch: %f, yaw: %f\n", roll, pitch, yaw);
#endif
for (int leg_no = 0; leg_no < num_legs; leg_no++) {
for (int joint_no = 0; joint_no < num_joints; joint_no++) {
dReal jangle = dJointGetHingeAngle(leg[segment][leg_no][joint_no].joint) - jointError[segment][leg_no][joint_no]; // hidding joint error for sensor
int currDirectionPositive;
if(jangle-lastJangle[segment][leg_no][joint_no] > 0.00001) currDirectionPositive =1; //we use a small positive number to ignore tinsy values
else currDirectionPositive =0;
//printf("jangle: %e, lastJangle_pneat[leg_no][joint_no]: %e, currDir: %i\n", jangle, lastJangle_pneat[leg_no][joint_no], currDirectionPositive);
if(printJointAngles){
ofstream jangle_file;
jangle_file.open ("jointAngles.txt", ios::app );
jangle_file << jangle << " ";
if(leg_no==num_legs-1 and joint_no==num_joints-1) jangle_file << endl;
jangle_file.close();
}
if(currDirectionPositive and !lastJangleDirectionPositve[segment][leg_no][joint_no]) numDirectionChanges++;
if(!currDirectionPositive and lastJangleDirectionPositve[segment][leg_no][joint_no]) numDirectionChanges++;
//if(visualize_pneat) printf("numDirectionChanges_pneat %i \n", numDirectionChanges_pneat);
lastJangleDirectionPositve[segment][leg_no][joint_no] =currDirectionPositive;
lastJangle[segment][leg_no][joint_no] = jangle;
int xInt =joint_no; //which node we will be setting or getting. x & y are within the sheet, z specifies which sheet (0=input, 1=hidden, 2=output)
int yInt =leg_no+(segment*2);
int zInt =0;
#if LEGSWING_ODE_DEBUG
printf("About to set input (x: %i,y: %i,z: %i) to joint angle: %f \n", xInt, yInt, zInt, jangle);
#endif
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)],jangle);
//<start> if inputting lastDesiredAngle
//xInt = xInt+1; //bump it one, cause the last desired angle goes into the input to the right of the current angle for that joint
//#if LEGSWING_ODE_DEBUG
//printf("about to set input (x: %i,y: %i,z: %i) to last desired joint angle: %f \n", xInt, yInt, zInt, newDesiredAngle[leg_no][joint_no]);
//#endif
//substrate_pointer->setValue(
// nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)],
// newDesiredAngle[segment][leg_no][joint_no]
// );
}
//set whether the leg is touching the ground
int xInt = 3; //putting touch just to the right of the 4th legs lastAngle
int yInt = leg_no+(segment*2);
int zInt = 0;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], thisLegIsTouching[segment][leg_no]);
}
#ifndef LEGSWING_ODE_DEBUG
assert(num_legs==4); //warning: only works with four legs
printf("Legs touching: FrontLeft (%i), BackLeft (%i), BackRight (%i), FrontRight (%i)\n", thisLegIsTouching[segment][0],thisLegIsTouching[segment][1], thisLegIsTouching[segment][2], thisLegIsTouching[segment][3]);
#endif
//printf("timeStepDivisor_pneat: %f\n", timeStepDivisor_pneat);
// use if evolving sine period
// if(fabs(timeStepDivisor_pneat<1)) sineOfTimeStep =0; //prevent divide by zero errors, and allow them to silence the sine wave
// else sineOfTimeStep = sin(dReal(timeSteps_pneat)/timeStepDivisor_pneat)*M_PI;
dReal sineOfTimeStep = sin(dReal(timeSteps)/50.0)*M_PI;
#ifdef USE_EXTRA_NODE
dReal negSineOfTimeStep = (-1.0 * sineOfTimeStep);
#endif
if(visualize and sineOfTimeStep>.9999*M_PI) printf("****************************************************\n");
if(visualize and sineOfTimeStep<-.9999*M_PI) printf("---------------------------------------------------\n");
//printf("sineOfTimeStep: %f\n", sineOfTimeStep);
#ifndef USE_NLEG_ROBOT
//<start> use to spread the PRYS horizontally
//insert the roll, pitch and yaw to the 7th-9th columns of the 1st row
//int yInt = 0;
//int zInt = 0;
//dReal insertValue;
//
//for(int q=0;q<3;q++){
// int xInt = 7+q;
// if (q==0) insertValue = roll;
// else if (q==1) insertValue = pitch;
// else insertValue = yaw;
// substrate_pointer->setValue(
// nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)],
// insertValue
// );
//}
//<ent> use to spread the PRYS horizontally
//<start> to spread PRYS vertically
//insert the roll, pitch and yaw to the 1st/0th-4th/3rd row of the 5th/4th columns
int xInt = 4;
int zInt = 0;
dReal insertValue;
for(int q=0;q<3;q++){
int yInt = q;
if (q==0) insertValue = roll;
else if (q==1) insertValue = pitch;
else insertValue = yaw;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], insertValue);
}
//<end>
//insert a sine wave based on timeSteps
xInt = 4;
int yInt = 3;
zInt = 0;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], sineOfTimeStep);
#else // USE_NLEG_ROBOT
//<start> to spread PRYS vertically
//insert the roll, pitch and yaw to the 1st/0th-4th/3rd row of the 5th/4th columns
int xInt = 4;
int zInt = 0;
dReal insertValue;
//roll
int yInt = segment*2;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], roll);
//pitch
yInt = (segment*2)+1;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], pitch);
//yaw
xInt = 5;
yInt = segment*2;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], yaw);
//<end>
if(segment < 1)
{
//insert a sine wave based on timeSteps
yInt = segment*2+1;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], sineOfTimeStep);
} else
{
dReal torsoAngle = dJointGetHingeAngle(torso[segment-1].joint); //TODO: does torso zero have a valid joint?
yInt = segment*2+1;
substrate_pointer->setValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)], torsoAngle);
}
#endif
#if SUBSTRATE_DEBUG
cout << "State of the nodes pre-update\n";
for (int z=0;z<numSheets;z++)
{
cout << "Printing Layer " << z << "************************************************" << endl;
for (int y1=0;y1<numNodesYglobal;y1++)
{
for (int x1=0;x1<numNodesXglobal;x1++)
{
//cout << "(x,y,z): (" << x1 << "," << y1 << "," << z << "): " ;
cout << setw(12) << setiosflags(ios::right) << fixed <<
double(substrate_pointer->getValue(
nameLookupGlobal[HCUBE::Node(x1,y1,z)]
));
}
cout << endl;
}
}
cout << "Updating Substrate\n";
//CREATE_PAUSE(string("Line Number: ")+toString(__LINE__));
#endif
//have to update the network once for each layer of *links* (or NumLayers -1)
#if SUBSTRATE_DEBUG
for (int a=0;a< numSheets - 1 ;a++)
{
substrate_pointer->update(1);
cout << "State of the nodes post-update: " << a << endl;
for (int z=0;z<numSheets;z++)
{
cout << "Printing Layer " << z << endl;
for (int y1=0;y1<numNodesYglobal;y1++)
{
for (int x1=0;x1<numNodesXglobal;x1++)
{
//cout << "(x,y,z): (" << x1 << "," << y1 << "," << z << "): " ;
cout << double(substrate_pointer->getValue(nameLookupGlobal[HCUBE::Node(x1,y1,z)]));
cout << " ";
}
cout << endl;
}
}
}
#else
substrate_pointer->update(numSheets-1);
#endif
}
for(int segment = 0; segment < BODY_SEGMENTS; ++segment){
for (int leg_no = 0; leg_no < num_legs; leg_no++) {
for (int joint_no = 0; joint_no < num_joints; joint_no++) {
int xInt =joint_no*2; //which node we will be setting or getting. x & y are within the sheet, z specifies which sheet (0=input, 1=hidden, 2=output)
int yInt =leg_no;
int zInt = numSheets-1; //2 (or numSheets - 1) to get the output
newDesiredAngle[segment][leg_no][joint_no] = M_PI*double(substrate_pointer->getValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)]));
#if LEGSWING_ODE_DEBUG
printf("newDesiredAngle is: %f\n", newDesiredAngle[segment][leg_no][joint_no]);
#endif
THETA[segment][leg_no][joint_no] = newDesiredAngle[segment][leg_no][joint_no] + jointError[segment][leg_no][joint_no]; // adding joint error
}
}
#ifdef USE_NLEG_ROBOT
if(segment < BODY_SEGMENTS-1) {
//get torso joint angle
int xInt = 5;
int yInt = segment*2+1;
int zInt = numSheets - 1;
torsoJointDesiredAngle[segment] = M_PI*double(substrate_pointer->getValue(nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)]));
}
#endif
//get the timeStepDivisor
// xInt =10; //which node we will be setting or getting. x & y are within the sheet, z specifies which sheet (0=input, 1=hidden, 2=output)
// yInt =1;
// zInt = 2; //2 to get the output
// timeStepDivisor = 100.0*double(substrate_pointer->getValue(
// nameLookupGlobal[HCUBE::Node(xInt,yInt,zInt)]));
//if(visualize)printf("timeStepDivisor!: %f\n", timeStepDivisor);
const dReal * torsoPosition = dBodyGetPosition(torso[segment].body);
if(fabs(torsoPosition[0]) > maxXdistanceFrom0[segment]){
maxXdistanceFrom0[segment] = fabs(torsoPosition[0]);
#ifndef LEGSWING_ODE_DEBUG
printf("new maxX::::::::::::::::::::::::::::::::::::::::::::::: %f\n", maxXdistanceFrom0);
#endif
}
if(fabs(torsoPosition[1]) > maxYdistanceFrom0[segment]){
maxYdistanceFrom0[segment] = fabs(torsoPosition[1]);
#ifndef LEGSWING_ODE_DEBUG
printf("new maxY:!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! %f\n", maxYdistanceFrom0);
#endif
}
currentXdistanceFrom0[segment] = fabs(torsoPosition[0]);
currentYdistanceFrom0[segment] = fabs(torsoPosition[1]);
}
Pcontrol();
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
for(int z=0;z<num_legs;z++) thisLegIsTouching[segment][z]=0; //reset these flags before checking the new state of things
}
if(fitness_function == 2) {
for(int segment = 0; segment < BODY_SEGMENTS; ++segment) {
for (int i = 0; i < num_legs; i++) {
for (int j = 0; j < num_links; j++) {
totalForce[0] += dBodyGetForce(leg[segment][i][j].body)[0];
totalForce[1] += dBodyGetForce(leg[segment][i][j].body)[1];
totalForce[2] += dBodyGetForce(leg[segment][i][j].body)[2];
}
}
totalForce[0] += dBodyGetForce(torso[segment].body)[0];
totalForce[1] += dBodyGetForce(torso[segment].body)[1];
totalForce[2] += dBodyGetForce(torso[segment].body)[2];
}
}
}
void UniversalJoint::createJointPhysics()
{
physicalJoint = dJointCreateUniversal(*simulation->getPhysicalWorld(), 0);
PhysicalObject* o1 = getPhysicalParentObject();
PhysicalObject* o2 = getPhysicalChildObject(this);
assert (o1 && o2);
dJointAttach(physicalJoint, *(o2->getBody()), *(o1->getBody()));
//set joint parameter
dJointSetUniversalAnchor(physicalJoint, dReal(anchorPoint.v[0]), dReal(anchorPoint.v[1]), dReal(anchorPoint.v[2]));
Vector3d physAxis(axis1->direction);
physAxis.rotate(rotation);
dJointSetUniversalAxis1(physicalJoint, dReal(physAxis.v[0]), dReal(physAxis.v[1]), dReal(physAxis.v[2]));
physAxis = axis2->direction;
physAxis.rotate(rotation);
dJointSetUniversalAxis2(physicalJoint, dReal(physAxis.v[0]), dReal(physAxis.v[1]), dReal(physAxis.v[2]));
if(axis1->cfm != -1.0)
dJointSetUniversalParam(physicalJoint, dParamCFM, dReal(axis1->cfm));
if(axis2->cfm != -1.0)
dJointSetUniversalParam(physicalJoint, dParamCFM2, dReal(axis2->cfm));
if(axis1->limited || (axis1->motor != 0 && axis1->motor->getMotorType() == ANGULAR))
{
double minAxisLimit(Functions::toRad(axis1->minValue));
double maxAxisLimit(Functions::toRad(axis1->maxValue));
//Set physical limits to higher values (+10%) to avoid strange hinge effects.
//Otherwise, sometimes the motor exceeds the limits.
assert(maxAxisLimit >= minAxisLimit);
double totalAxisRange(maxAxisLimit - minAxisLimit);
double internalTolerance(totalAxisRange / 10);
minAxisLimit -= internalTolerance;
maxAxisLimit += internalTolerance;
dJointSetUniversalParam(physicalJoint, dParamLoStop, dReal(minAxisLimit));
dJointSetUniversalParam(physicalJoint, dParamHiStop, dReal(maxAxisLimit));
// this has to be done due to the way ODE sets joint stops
dJointSetUniversalParam(physicalJoint, dParamLoStop, dReal(minAxisLimit));
if(axis1->fullScaleDeflectionCFM != -1.0)
dJointSetUniversalParam(physicalJoint, dParamStopCFM, dReal(axis1->fullScaleDeflectionCFM));
if(axis1->fullScaleDeflectionERP != -1.0)
dJointSetUniversalParam(physicalJoint, dParamStopERP, dReal(axis1->fullScaleDeflectionERP));
}
if(axis2->limited || (axis2->motor != 0 && axis2->motor->getMotorType() == ANGULAR))
{
double minAxisLimit(Functions::toRad(axis2->minValue));
double maxAxisLimit(Functions::toRad(axis2->maxValue));
//Set physical limits to higher values (+10%) to avoid strange hinge effects.
//Otherwise, sometimes the motor exceeds the limits.
assert(maxAxisLimit >= minAxisLimit);
double totalAxisRange(maxAxisLimit - minAxisLimit);
double internalTolerance(totalAxisRange / 10);
minAxisLimit -= internalTolerance;
maxAxisLimit += internalTolerance;
dJointSetUniversalParam(physicalJoint, dParamLoStop2, dReal(minAxisLimit));
dJointSetUniversalParam(physicalJoint, dParamHiStop2, dReal(maxAxisLimit));
// this has to be done due to the way ODE sets joint stops
dJointSetUniversalParam(physicalJoint, dParamLoStop2, dReal(minAxisLimit));
if(axis2->fullScaleDeflectionCFM != -1.0)
dJointSetUniversalParam(physicalJoint, dParamStopCFM2, dReal(axis2->fullScaleDeflectionCFM));
if(axis2->fullScaleDeflectionERP != -1.0)
dJointSetUniversalParam(physicalJoint, dParamStopERP2, dReal(axis2->fullScaleDeflectionERP));
}
}
int dCollideCylinderPlane(dxGeom *Cylinder, dxGeom *Plane, int flags, dContactGeom *contact, int skip)
{
dIASSERT (skip >= (int)sizeof(dContactGeom));
dIASSERT ((flags & 0xffff) >= 1);
unsigned char* pContactData = (unsigned char*)contact;
int GeomCount = 0; // count of used contactgeoms
#ifdef dSINGLE
const dReal toleranz = 0.0001f;
#endif
#ifdef dDOUBLE
const dReal toleranz = 0.0000001;
#endif
// Get the properties of the cylinder (length+radius)
dReal radius, length;
dGeomCylinderGetParams(Cylinder, &radius, &length);
dVector3 &cylpos = Cylinder->final_posr->pos;
// and the plane
dVector4 planevec;
dGeomPlaneGetParams(Plane, planevec);
dVector3 PlaneNormal = {planevec[0],planevec[1],planevec[2]};
dVector3 PlanePos = {planevec[0] * planevec[3],planevec[1] * planevec[3],planevec[2] * planevec[3]};
dVector3 G1Pos1, G1Pos2, vDir1;
vDir1[0] = Cylinder->final_posr->R[2];
vDir1[1] = Cylinder->final_posr->R[6];
vDir1[2] = Cylinder->final_posr->R[10];
dReal s;
s = length * dReal(0.5);
G1Pos2[0] = vDir1[0] * s + cylpos[0];
G1Pos2[1] = vDir1[1] * s + cylpos[1];
G1Pos2[2] = vDir1[2] * s + cylpos[2];
G1Pos1[0] = vDir1[0] * -s + cylpos[0];
G1Pos1[1] = vDir1[1] * -s + cylpos[1];
G1Pos1[2] = vDir1[2] * -s + cylpos[2];
dVector3 C;
// parallel-check
s = vDir1[0] * PlaneNormal[0] + vDir1[1] * PlaneNormal[1] + vDir1[2] * PlaneNormal[2];
if(s < 0)
s += dReal(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel
else
s -= dReal(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel
if(s < toleranz && s > (-toleranz))
{
// discs are parallel to the plane
// 1.compute if, and where contacts are
dVector3 P;
s = planevec[3] - planevec[0] * G1Pos1[0] - planevec[1] * G1Pos1[1] - planevec[2] * G1Pos1[2];
dReal t;
t = planevec[3] - planevec[0] * G1Pos2[0] - planevec[1] * G1Pos2[1] - planevec[2] * G1Pos2[2];
if(s >= t) // s == t does never happen,
{
if(s >= 0)
{
// 1. Disc
P[0] = G1Pos1[0];
P[1] = G1Pos1[1];
P[2] = G1Pos1[2];
}
else
return GeomCount; // no contacts
}
else
{
if(t >= 0)
{
// 2. Disc
P[0] = G1Pos2[0];
P[1] = G1Pos2[1];
P[2] = G1Pos2[2];
}
else
return GeomCount; // no contacts
}
// 2. generate a coordinate-system on the disc
dVector3 V1, V2;
if(vDir1[0] < toleranz && vDir1[0] > (-toleranz))
{
// not x-axis
V1[0] = vDir1[0] + dReal(1.0); // random value
V1[1] = vDir1[1];
V1[2] = vDir1[2];
}
else
{
// maybe x-axis
V1[0] = vDir1[0];
V1[1] = vDir1[1] + dReal(1.0); // random value
V1[2] = vDir1[2];
}
// V1 is now another direction than vDir1
// Cross-product
V2[0] = V1[1] * vDir1[2] - V1[2] * vDir1[1];
V2[1] = V1[2] * vDir1[0] - V1[0] * vDir1[2];
V2[2] = V1[0] * vDir1[1] - V1[1] * vDir1[0];
// make unit V2
t = dReal(sqrt(V2[0] * V2[0] + V2[1] * V2[1] + V2[2] * V2[2]));
t = radius / t;
V2[0] *= t;
V2[1] *= t;
V2[2] *= t;
// cross again
V1[0] = V2[1] * vDir1[2] - V2[2] * vDir1[1];
V1[1] = V2[2] * vDir1[0] - V2[0] * vDir1[2];
V1[2] = V2[0] * vDir1[1] - V2[1] * vDir1[0];
// |V2| is 'radius' and vDir1 unit, so |V1| is 'radius'
// V1 = first axis
// V2 = second axis
// 3. generate contactpoints
// Potential contact 1
contact->pos[0] = P[0] + V1[0];
contact->pos[1] = P[1] + V1[1];
contact->pos[2] = P[2] + V1[2];
contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2];
if(contact->depth > 0)
{
contact->normal[0] = PlaneNormal[0];
contact->normal[1] = PlaneNormal[1];
contact->normal[2] = PlaneNormal[2];
contact->g1 = Cylinder;
contact->g2 = Plane;
GeomCount++;
if( GeomCount >= (flags & 0x0ffff))
return GeomCount; // enough contactgeoms
pContactData += skip;
contact = (dContactGeom*)pContactData;
}
// Potential contact 2
contact->pos[0] = P[0] - V1[0];
contact->pos[1] = P[1] - V1[1];
contact->pos[2] = P[2] - V1[2];
contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2];
if(contact->depth > 0)
{
contact->normal[0] = PlaneNormal[0];
contact->normal[1] = PlaneNormal[1];
contact->normal[2] = PlaneNormal[2];
contact->g1 = Cylinder;
contact->g2 = Plane;
GeomCount++;
if( GeomCount >= (flags & 0x0ffff))
return GeomCount; // enough contactgeoms
pContactData += skip;
contact = (dContactGeom*)pContactData;
}
// Potential contact 3
contact->pos[0] = P[0] + V2[0];
contact->pos[1] = P[1] + V2[1];
contact->pos[2] = P[2] + V2[2];
contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2];
if(contact->depth > 0)
{
contact->normal[0] = PlaneNormal[0];
contact->normal[1] = PlaneNormal[1];
contact->normal[2] = PlaneNormal[2];
contact->g1 = Cylinder;
contact->g2 = Plane;
GeomCount++;
if( GeomCount >= (flags & 0x0ffff))
return GeomCount; // enough contactgeoms
pContactData += skip;
contact = (dContactGeom*)pContactData;
}
// Potential contact 4
contact->pos[0] = P[0] - V2[0];
contact->pos[1] = P[1] - V2[1];
contact->pos[2] = P[2] - V2[2];
contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2];
if(contact->depth > 0)
{
contact->normal[0] = PlaneNormal[0];
contact->normal[1] = PlaneNormal[1];
contact->normal[2] = PlaneNormal[2];
contact->g1 = Cylinder;
contact->g2 = Plane;
GeomCount++;
if( GeomCount >= (flags & 0x0ffff))
return GeomCount; // enough contactgeoms
pContactData += skip;
contact = (dContactGeom*)pContactData;
}
}
else
{
dReal t = -((-PlaneNormal[0]) * vDir1[0] + (-PlaneNormal[1]) * vDir1[1] + (-PlaneNormal[2]) * vDir1[2]);
C[0] = vDir1[0] * t - PlaneNormal[0];
C[1] = vDir1[1] * t - PlaneNormal[1];
C[2] = vDir1[2] * t - PlaneNormal[2];
s = dReal(sqrt(C[0] * C[0] + C[1] * C[1] + C[2] * C[2]));
// move C onto the circle
s = radius / s;
C[0] *= s;
C[1] *= s;
C[2] *= s;
// deepest point of disc 1
contact->pos[0] = C[0] + G1Pos1[0];
contact->pos[1] = C[1] + G1Pos1[1];
contact->pos[2] = C[2] + G1Pos1[2];
// depth of the deepest point
contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2];
if(contact->depth >= 0)
{
contact->normal[0] = PlaneNormal[0];
contact->normal[1] = PlaneNormal[1];
contact->normal[2] = PlaneNormal[2];
contact->g1 = Cylinder;
contact->g2 = Plane;
GeomCount++;
if( GeomCount >= (flags & 0x0ffff))
return GeomCount; // enough contactgeoms
pContactData += skip;
contact = (dContactGeom*)pContactData;
}
// C is still computed
// deepest point of disc 2
contact->pos[0] = C[0] + G1Pos2[0];
contact->pos[1] = C[1] + G1Pos2[1];
contact->pos[2] = C[2] + G1Pos2[2];
// depth of the deepest point
contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2];
if(contact->depth >= 0)
{
contact->normal[0] = PlaneNormal[0];
contact->normal[1] = PlaneNormal[1];
contact->normal[2] = PlaneNormal[2];
contact->g1 = Cylinder;
contact->g2 = Plane;
GeomCount++;
if( GeomCount >= (flags & 0x0ffff))
return GeomCount; // enough contactgeoms
pContactData += skip;
contact = (dContactGeom*)pContactData;
}
}
return GeomCount;
}
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 nearCallback(void *, dGeomID a, dGeomID b)
{
const unsigned max_contacts = 8;
dContact contacts[max_contacts];
if (!dGeomGetBody(a) && !dGeomGetBody(b))
return; // don't handle static geom collisions
int n = dCollide(a, b, max_contacts, &contacts[0].geom, sizeof(dContact));
//clog << "got " << n << " contacts" << endl;
/* Simple contact merging:
* If we have contacts that are too close with the same normal, keep only
* the one with maximum depth.
* The epsilon that defines what "too close" means can be a heuristic.
*/
int new_n = 0;
dReal epsilon = 1e-1; // default
/* If we know one of the geoms is a sphere, we can base the epsilon on the
* sphere's radius.
*/
dGeomID s = 0;
if ((dGeomGetClass(a) == dSphereClass && (s = a)) ||
(dGeomGetClass(b) == dSphereClass && (s = b))) {
epsilon = dGeomSphereGetRadius(s) * 0.3;
}
for (int i=0; i<n; ++i) {
// this block draws the contact points before merging, in red
dMatrix3 r;
dRSetIdentity(r);
dsSetColor(1, 0, 0);
dsSetTexture(DS_NONE);
dsDrawSphere(contacts[i].geom.pos, r, 0.008);
// let's offset the line a bit to avoid drawing overlap issues
float xyzf[3], hprf[3];
dsGetViewpoint(xyzf, hprf);
dVector3 xyz = {dReal(xyzf[0]), dReal(xyzf[1]), dReal(xyzf[2])};
dVector3 v;
dSubtractVectors3(v, contacts[i].geom.pos, xyz);
dVector3 c;
dCalcVectorCross3(c, v, contacts[i].geom.pos);
dSafeNormalize3(c);
dVector3 pos1;
dAddScaledVectors3(pos1, contacts[i].geom.pos, c, 1, 0.005);
dVector3 pos2;
dAddScaledVectors3(pos2, pos1, contacts[i].geom.normal, 1, 0.05);
dsDrawLine(pos1, pos2);
// end of contacts drawing code
int closest_point = i;
for (int j=0; j<new_n; ++j) {
dReal alignment = dCalcVectorDot3(contacts[i].geom.normal, contacts[j].geom.normal);
if (alignment > 0.99 // about 8 degrees of difference
&&
dCalcPointsDistance3(contacts[i].geom.pos, contacts[j].geom.pos) < epsilon) {
// they are too close
closest_point = j;
//clog << "found close points: " << j << " and " << i << endl;
break;
}
}
if (closest_point != i) {
// we discard one of the points
if (contacts[i].geom.depth > contacts[closest_point].geom.depth)
// the new point is deeper, copy it over closest_point
contacts[closest_point] = contacts[i];
} else
contacts[new_n++] = contacts[i]; // the point is preserved
}
//clog << "reduced from " << n << " to " << new_n << endl;
n = new_n;
for (int i=0; i<n; ++i) {
contacts[i].surface.mode = dContactBounce | dContactApprox1 | dContactSoftERP;
contacts[i].surface.mu = 10;
contacts[i].surface.bounce = 0.2;
contacts[i].surface.bounce_vel = 0;
contacts[i].surface.soft_erp = 1e-3;
//clog << "depth: " << contacts[i].geom.depth << endl;
dJointID contact = dJointCreateContact(world, contact_group, &contacts[i]);
dJointAttach(contact, dGeomGetBody(a), dGeomGetBody(b));
dMatrix3 r;
dRSetIdentity(r);
dsSetColor(0, 0, 1);
dsSetTexture(DS_NONE);
dsDrawSphere(contacts[i].geom.pos, r, 0.01);
dsSetColor(0, 1, 0);
dVector3 pos2;
dAddScaledVectors3(pos2, contacts[i].geom.pos, contacts[i].geom.normal, 1, 0.1);
dsDrawLine(contacts[i].geom.pos, pos2);
}
//clog << "----" << endl;
}
void _dLDLTAddTL (dReal *L, dReal *d, const dReal *a, int n, int nskip, void *tmpbuf/*[2*nskip]*/)
{
dAASSERT (L && d && a && n > 0 && nskip >= n);
if (n < 2) return;
dReal *W1 = tmpbuf ? (dReal *)tmpbuf : (dReal*) ALLOCA ((2*nskip)*sizeof(dReal));
dReal *W2 = W1 + nskip;
W1[0] = REAL(0.0);
W2[0] = REAL(0.0);
for (int j=1; j<n; ++j) {
W1[j] = W2[j] = (dReal) (a[j] * M_SQRT1_2);
}
dReal W11 = (dReal) ((REAL(0.5)*a[0]+1)*M_SQRT1_2);
dReal W21 = (dReal) ((REAL(0.5)*a[0]-1)*M_SQRT1_2);
dReal alpha1 = REAL(1.0);
dReal alpha2 = REAL(1.0);
{
dReal dee = d[0];
dReal alphanew = alpha1 + (W11*W11)*dee;
dIASSERT(alphanew != dReal(0.0));
dee /= alphanew;
dReal gamma1 = W11 * dee;
dee *= alpha1;
alpha1 = alphanew;
alphanew = alpha2 - (W21*W21)*dee;
dee /= alphanew;
dReal gamma2 = W21 * dee;
alpha2 = alphanew;
dReal k1 = REAL(1.0) - W21*gamma1;
dReal k2 = W21*gamma1*W11 - W21;
dReal *ll = L + nskip;
for (int p=1; p<n; ll+=nskip, ++p) {
dReal Wp = W1[p];
dReal ell = *ll;
W1[p] = Wp - W11*ell;
W2[p] = k1*Wp + k2*ell;
}
}
dReal *ll = L + (nskip + 1);
for (int j=1; j<n; ll+=nskip+1, ++j) {
dReal k1 = W1[j];
dReal k2 = W2[j];
dReal dee = d[j];
dReal alphanew = alpha1 + (k1*k1)*dee;
dIASSERT(alphanew != dReal(0.0));
dee /= alphanew;
dReal gamma1 = k1 * dee;
dee *= alpha1;
alpha1 = alphanew;
alphanew = alpha2 - (k2*k2)*dee;
dee /= alphanew;
dReal gamma2 = k2 * dee;
dee *= alpha2;
d[j] = dee;
alpha2 = alphanew;
dReal *l = ll + nskip;
for (int p=j+1; p<n; l+=nskip, ++p) {
dReal ell = *l;
dReal Wp = W1[p] - k1 * ell;
ell += gamma1 * Wp;
W1[p] = Wp;
Wp = W2[p] - k2 * ell;
ell -= gamma2 * Wp;
W2[p] = Wp;
*l = ell;
}
}
}
void dInternalHandleAutoDisabling (dxWorld *world, dReal stepsize)
{
dxBody *bb;
for ( bb=world->firstbody; bb; bb=(dxBody*)bb->next )
{
// don't freeze objects mid-air (patch 1586738)
if ( bb->firstjoint == NULL ) continue;
// nothing to do unless this body is currently enabled and has
// the auto-disable flag set
if ( (bb->flags & (dxBodyAutoDisable|dxBodyDisabled)) != dxBodyAutoDisable ) continue;
// if sampling / threshold testing is disabled, we can never sleep.
if ( bb->adis.average_samples == 0 ) continue;
//
// see if the body is idle
//
#ifndef dNODEBUG
// sanity check
if ( bb->average_counter >= bb->adis.average_samples )
{
dUASSERT( bb->average_counter < bb->adis.average_samples, "buffer overflow" );
// something is going wrong, reset the average-calculations
bb->average_ready = 0; // not ready for average calculation
bb->average_counter = 0; // reset the buffer index
}
#endif // dNODEBUG
// sample the linear and angular velocity
bb->average_lvel_buffer[bb->average_counter][0] = bb->lvel[0];
bb->average_lvel_buffer[bb->average_counter][1] = bb->lvel[1];
bb->average_lvel_buffer[bb->average_counter][2] = bb->lvel[2];
bb->average_avel_buffer[bb->average_counter][0] = bb->avel[0];
bb->average_avel_buffer[bb->average_counter][1] = bb->avel[1];
bb->average_avel_buffer[bb->average_counter][2] = bb->avel[2];
bb->average_counter++;
// buffer ready test
if ( bb->average_counter >= bb->adis.average_samples )
{
bb->average_counter = 0; // fill the buffer from the beginning
bb->average_ready = 1; // this body is ready now for average calculation
}
int idle = 0; // Assume it's in motion unless we have samples to disprove it.
// enough samples?
if ( bb->average_ready )
{
idle = 1; // Initial assumption: IDLE
// the sample buffers are filled and ready for calculation
dVector3 average_lvel, average_avel;
// Store first velocity samples
average_lvel[0] = bb->average_lvel_buffer[0][0];
average_avel[0] = bb->average_avel_buffer[0][0];
average_lvel[1] = bb->average_lvel_buffer[0][1];
average_avel[1] = bb->average_avel_buffer[0][1];
average_lvel[2] = bb->average_lvel_buffer[0][2];
average_avel[2] = bb->average_avel_buffer[0][2];
// If we're not in "instantaneous mode"
if ( bb->adis.average_samples > 1 )
{
// add remaining velocities together
for ( unsigned int i = 1; i < bb->adis.average_samples; ++i )
{
average_lvel[0] += bb->average_lvel_buffer[i][0];
average_avel[0] += bb->average_avel_buffer[i][0];
average_lvel[1] += bb->average_lvel_buffer[i][1];
average_avel[1] += bb->average_avel_buffer[i][1];
average_lvel[2] += bb->average_lvel_buffer[i][2];
average_avel[2] += bb->average_avel_buffer[i][2];
}
// make average
dReal r1 = dReal( 1.0 ) / dReal( bb->adis.average_samples );
average_lvel[0] *= r1;
average_avel[0] *= r1;
average_lvel[1] *= r1;
average_avel[1] *= r1;
average_lvel[2] *= r1;
average_avel[2] *= r1;
}
// threshold test
dReal av_lspeed, av_aspeed;
av_lspeed = dCalcVectorDot3( average_lvel, average_lvel );
if ( av_lspeed > bb->adis.linear_average_threshold )
{
idle = 0; // average linear velocity is too high for idle
}
else
{
av_aspeed = dCalcVectorDot3( average_avel, average_avel );
if ( av_aspeed > bb->adis.angular_average_threshold )
{
idle = 0; // average angular velocity is too high for idle
}
}
}
// if it's idle, accumulate steps and time.
// these counters won't overflow because this code doesn't run for disabled bodies.
if (idle) {
bb->adis_stepsleft--;
bb->adis_timeleft -= stepsize;
}
else {
// Reset countdowns
bb->adis_stepsleft = bb->adis.idle_steps;
bb->adis_timeleft = bb->adis.idle_time;
}
// disable the body if it's idle for a long enough time
if ( bb->adis_stepsleft <= 0 && bb->adis_timeleft <= 0 )
{
bb->flags |= dxBodyDisabled; // set the disable flag
// disabling bodies should also include resetting the velocity
// should prevent jittering in big "islands"
bb->lvel[0] = 0;
bb->lvel[1] = 0;
bb->lvel[2] = 0;
bb->avel[0] = 0;
bb->avel[1] = 0;
bb->avel[2] = 0;
}
}
}
void cullPoints (int n, dReal p[], int m, int i0, int iret[])
{
// compute the centroid of the polygon in cx,cy
int i,j;
dReal a,cx,cy,q;
if (n==1) {
cx = p[0];
cy = p[1];
}
else if (n==2) {
cx = REAL(0.5)*(p[0] + p[2]);
cy = REAL(0.5)*(p[1] + p[3]);
}
else {
a = 0;
cx = 0;
cy = 0;
for (i=0; i<(n-1); i++) {
q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
a += q;
cx += q*(p[i*2]+p[i*2+2]);
cy += q*(p[i*2+1]+p[i*2+3]);
}
q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
a = dRecip(REAL(3.0)*(a+q));
cx = a*(cx + q*(p[n*2-2]+p[0]));
cy = a*(cy + q*(p[n*2-1]+p[1]));
}
// compute the angle of each point w.r.t. the centroid
dReal A[8];
for (i=0; i<n; i++) A[i] = dAtan2(p[i*2+1]-cy,p[i*2]-cx);
// search for points that have angles closest to A[i0] + i*(2*pi/m).
int avail[8];
for (i=0; i<n; i++) avail[i] = 1;
avail[i0] = 0;
iret[0] = i0;
iret++;
for (j=1; j<m; j++) {
a = (dReal)(dReal(j)*(2*M_PI/m) + A[i0]);
if (a > M_PI) a -= (dReal)(2*M_PI);
dReal maxdiff=1e9,diff;
#ifndef dNODEBUG
*iret = i0; // iret is not allowed to keep this value
#endif
for (i=0; i<n; i++) {
if (avail[i]) {
diff = dFabs (A[i]-a);
if (diff > M_PI) diff = (dReal) (2*M_PI - diff);
if (diff < maxdiff) {
maxdiff = diff;
*iret = i;
}
}
}
#ifndef dNODEBUG
dIASSERT (*iret != i0); // ensure iret got set
#endif
avail[*iret] = 0;
iret++;
}
}
void VelocityMotor::act()
{
dJointSetHingeParam(joint->joint, dParamVel, dReal(setpoint));
}
