#include <assert.h>
#include <selforg/matrix.h>
#include <osg/Matrix>
#include "sphererobot3masses.h"
#include "irsensor.h"
#include "invisibleprimitive.h"
#include "osgprimitive.h"
#include "mathutils.h"
using namespace osg;
using namespace std;
namespace lpzrobots {
void Sphererobot3MassesConf::destroy(){
for(list<Sensor*>::iterator i = sensors.begin(); i != sensors.end(); i++){
if(*i) delete *i;
}
sensors.clear();
}
const int Sphererobot3Masses::servono;
Sphererobot3Masses::Sphererobot3Masses ( const OdeHandle& odeHandle, const OsgHandle& osgHandle,
const Sphererobot3MassesConf& conf, const std::string& name,
double transparency)
: OdeRobot ( odeHandle, osgHandle, name, "$Id: sphererobot3masses.cpp,v 1.8 2007/01/03 15:01:09 fhesse Exp $"),
conf(conf), transparency(transparency)
{
numberaxis=3;
init();
}
Sphererobot3Masses::Sphererobot3Masses ( const OdeHandle& odeHandle, const OsgHandle& osgHandle,
const Sphererobot3MassesConf& conf, const std::string& name,
const std::string& revision,
double transparency)
: OdeRobot ( odeHandle, osgHandle, name, revision), conf(conf),transparency(transparency)
{
numberaxis=3;
init();
}
void Sphererobot3Masses::init(){
created = false;
memset(object, 0,sizeof(void*) * Last);
memset(joint, 0,sizeof(void*) * servono);
memset(axis, 0,sizeof(void*) * servono);
memset(servo, 0,sizeof(void*) * servono);
this->conf.pendulardiameter = conf.diameter/7;
}
Sphererobot3Masses::~Sphererobot3Masses()
{
destroy();
}
void Sphererobot3Masses::update()
{
for (int i=0; i < Last; i++) {
if(object[i]) object[i]->update();
}
Matrix pose(object[Base]->getPose());
for (int i=0; i < servono; i++) {
if(axis[i]){
axis[i]->setMatrix(Matrix::rotate(M_PI/2, (i==1), (i==0), (i==2)) * pose);
}
}
irSensorBank.update();
}
int Sphererobot3Masses::getSensors ( sensor* sensors, int sensornumber )
{
int len=0;
assert(created);
matrix::Matrix A = odeRto3x3RotationMatrix ( dBodyGetRotation ( object[Base]->getBody() ) );
FOREACH(list<Sensor*>, conf.sensors, i){
len += (*i)->get(sensors+len, sensornumber-len);
}
if(conf.motorsensor){
for ( unsigned int n = 0; n < numberaxis; n++ ) {
sensors[len] = servo[n]->get() * 0.2;
len++;
}
}
if (conf.irAxis1 || conf.irAxis2 || conf.irAxis3){
len += irSensorBank.get(sensors+len, sensornumber-len);
}
return len;
}
void Sphererobot3Masses::setMotors ( const motor* motors, int motornumber ) {
int len = min((unsigned)motornumber, numberaxis);
for ( int n = 0; n < len; n++ ) {
servo[n]->set(motors[n]);
}
}
void Sphererobot3Masses::place(const osg::Matrix& pose){
osg::Matrix p2;
p2 = pose * osg::Matrix::translate(osg::Vec3(0, 0, conf.diameter/2));
create(p2);
};
void Sphererobot3Masses::doInternalStuff(const GlobalData& global){
irSensorBank.reset();
}
bool Sphererobot3Masses::collisionCallback(void *data, dGeomID o1, dGeomID o2) {
if( o1 == (dGeomID)odeHandle.space || o2 == (dGeomID)odeHandle.space) {
if(o1 == (dGeomID)odeHandle.space) irSensorBank.sense(o2);
if(o2 == (dGeomID)odeHandle.space) irSensorBank.sense(o1);
int i,n;
const int N = 40;
dContact contact[N];
n = dCollide (o1,o2,N,&contact[0].geom,sizeof(dContact));
for (i=0; i<n; i++) {
if( contact[i].geom.g1 == object[Base]->getGeom() || contact[i].geom.g2 == object[Base]->getGeom() ){
contact[i].surface.mode = dContactSlip1 | dContactSlip2 | dContactApprox1;
contact[i].surface.mu = 2.0;
contact[i].surface.slip1 = 0.005;
contact[i].surface.slip2 = 0.005;
dJointID c = dJointCreateContact( odeHandle.world, odeHandle.jointGroup, &contact[i]);
dJointAttach ( c , dGeomGetBody(contact[i].geom.g1) , dGeomGetBody(contact[i].geom.g2));
}
}
return true;
} else {
return false;
}
}
int Sphererobot3Masses::getMotorNumber(){
return numberaxis;
}
int Sphererobot3Masses::getSensorNumber() {
int s=0;
FOREACHC(list<Sensor*>, conf.sensors, i){
s += (*i)->getSensorNumber();
}
return conf.motorsensor * numberaxis + s +
(conf.irAxis1 + conf.irAxis2 + conf.irAxis3) * 2;
}
void Sphererobot3Masses::create(const osg::Matrix& pose){
if (created) {
destroy();
}
odeHandle.space = dSimpleSpaceCreate (parentspace);
Color c(osgHandle.color);
c.alpha() = transparency;
OsgHandle osgHandle_Base = osgHandle.changeColor(c);
OsgHandle osgHandleX[3];
osgHandleX[0] = osgHandle.changeColor(Color(1.0, 0.0, 0.0));
osgHandleX[1] = osgHandle.changeColor(Color(0.0, 1.0, 0.0));
osgHandleX[2] = osgHandle.changeColor(Color(0.0, 0.0, 1.0));
object[Base] = new Sphere(conf.diameter/2);
object[Base]->init(odeHandle, conf.spheremass, osgHandle_Base);
object[Base]->setPose(pose);
Pos p(pose.getTrans());
Primitive* pendular[servono];
memset(pendular, 0, sizeof(void*) * servono);
for ( unsigned int n = 0; n < numberaxis; n++ ) {
pendular[n] = new Sphere(conf.pendulardiameter/2);
pendular[n]->init(odeHandle, conf.pendularmass, osgHandleX[n],
Primitive::Body | Primitive::Draw);
pendular[n]->setPose(pose);
joint[n] = new SliderJoint(object[Base], pendular[n],
p, Axis((n==0), (n==1), (n==2))*pose);
joint[n]->init(odeHandle, osgHandle, false);
joint[n]->setParam ( dParamLoStop, -1.1*conf.diameter*conf.pendularrange );
joint[n]->setParam ( dParamHiStop, 1.1*conf.diameter*conf.pendularrange );
joint[n]->setParam ( dParamStopCFM, 0.1);
joint[n]->setParam ( dParamStopERP, 0.9);
joint[n]->setParam ( dParamCFM, 0.001);
servo[n] = new SliderServo(joint[n],
-conf.diameter*conf.pendularrange,
conf.diameter*conf.pendularrange,
conf.pendularmass);
axis[n] = new OSGCylinder(conf.diameter/100, conf.diameter - conf.diameter/100);
axis[n]->init(osgHandleX[n], OSGPrimitive::Low);
}
object[Pendular1] = pendular[0];
object[Pendular2] = pendular[1];
object[Pendular3] = pendular[2];
double sensorrange = conf.irsensorscale * conf.diameter;
RaySensor::rayDrawMode drawMode = conf.drawIRs ? RaySensor::drawAll : RaySensor::drawSensor;
irSensorBank.init(odeHandle, osgHandle );
if (conf.irAxis1){
for(int i=-1; i<2; i+=2){
IRSensor* sensor = new IRSensor(1.5);
Matrix R = Matrix::rotate(i*M_PI/2, 1, 0, 0) * Matrix::translate(0,-i*conf.diameter/2,0 );
irSensorBank.registerSensor(sensor, object[Base], R, sensorrange, drawMode);
}
}
if (conf.irAxis2){
for(int i=-1; i<2; i+=2){
IRSensor* sensor = new IRSensor(1.5);
Matrix R = Matrix::rotate(i*M_PI/2, 0, 1, 0) * Matrix::translate(i*conf.diameter/2,0,0 );
irSensorBank.registerSensor(sensor, object[Base], R, sensorrange, drawMode);
}
}
if (conf.irAxis3){
for(int i=-1; i<2; i+=2){
IRSensor* sensor = new IRSensor(1.5);
Matrix R = Matrix::rotate( i==1 ? 0 : M_PI, 1, 0, 0) * Matrix::translate(0,0,i*conf.diameter/2);
irSensorBank.registerSensor(sensor, object[Base], R, sensorrange, drawMode);
}
}
FOREACH(list<Sensor*>, conf.sensors, i){
(*i)->init(object[Base]);
}
created=true;
}
void Sphererobot3Masses::destroy(){
if (created){
for (int i=0; i<Last; i++){
if(object[i]) delete object[i];
}
for (int i=0; i<servono; i++){
if(joint[i]) delete joint[i];
if(servo[i]) delete servo[i];
if(axis[i]) delete axis[i];
}
irSensorBank.clear();
dSpaceDestroy(odeHandle.space);
}
created=false;
}
}