#include <assert.h>
#include <selforg/matrix.h>
#include <osg/Matrix>
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$"),
conf(conf), transparency(transparency)
{
numberaxis=3;
init();
addParameter("pendularrange",&this->conf.pendularrange,0,0.4,
"range of the masses along the sliders");
}
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;
objects.resize(Last);
joints.resize(servono);
memset(axis, 0,sizeof(void*) * servono);
memset(servo, 0,sizeof(void*) * servono);
this->conf.pendulardiameter = conf.diameter/7;
}
Sphererobot3Masses::~Sphererobot3Masses()
{
destroy();
if(conf.irSensorTempl) delete conf.irSensorTempl;
FOREACH(std::list<Sensor*>, conf.sensors, s){
if(*s) delete *s;
}
}
void Sphererobot3Masses::update()
{
for (int i=0; i < Last; i++) {
if(objects[i]) objects[i]->update();
}
Matrix pose(objects[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)) * Matrix::translate(0 ,0, (i==0?-1:1)*conf.axesShift)* pose);
}
}
irSensorBank.update();
FOREACH(std::list<Sensor*>, conf.sensors, s){
(*s)->update();
}
}
int Sphererobot3Masses::getSensorsIntern(
sensor* sensors,
int sensornumber )
{
int len=0;
assert(created);
if(!conf.motor_ir_before_sensors){
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.5;
len++;
}
}
if (conf.irAxis1 || conf.irAxis2 || conf.irAxis3 || conf.irRing || conf.irSide){
len += irSensorBank.get(sensors+len, sensornumber-len);
}
if(conf.motor_ir_before_sensors){
FOREACH(list<Sensor*>, conf.sensors, i){
len += (*i)->get(sensors+len, sensornumber-len);
}
}
return len;
}
void Sphererobot3Masses::setMotorsIntern( const double* motors, int motornumber ) {
int len =
min((
unsigned)motornumber, numberaxis);
for ( int n = 0; n < len; n++ ) {
servo[n]->set(motors[n]);
}
}
void Sphererobot3Masses::placeIntern(
const osg::Matrix& pose){
p2 = pose * osg::Matrix::translate(
osg::Vec3(0, 0, conf.diameter/2));
create(p2);
};
void Sphererobot3Masses::sense(GlobalData& globalData) {
irSensorBank.sense(globalData);
OdeRobot::sense(globalData);
}
void Sphererobot3Masses::doInternalStuff(GlobalData& global){
OdeRobot::doInternalStuff(global);
dBodyID b = getMainPrimitive()->getBody();
double friction = odeHandle.substance.roughness;
const double* vel = dBodyGetAngularVel( b);
if(fabs(vel[2])>0.2){
dBodyAddTorque ( b , 0 , 0 , -0.05*friction*vel[2] );
}
if(conf.brake){
dBodyAddTorque ( b , -conf.brake*vel[0] , -conf.brake*vel[1] , -conf.brake*vel[2] );
}
FOREACH(list<Sensor*>, conf.sensors, s){
(*s)->sense(global);
}
}
int Sphererobot3Masses::getMotorNumberIntern(){
return numberaxis;
}
int Sphererobot3Masses::getSensorNumberIntern() {
int s=0;
FOREACHC(list<Sensor*>, conf.sensors, i){
s += (*i)->getSensorNumber();
}
return conf.motorsensor * numberaxis + s + irSensorBank.getSensorNumber();
}
void Sphererobot3Masses::create(
const osg::Matrix& pose){
if (created) {
destroy();
}
odeHandle.createNewSimpleSpace(parentspace,true);
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));
objects[Base] = new Sphere(conf.diameter/2);
objects[Base]->init(odeHandle, conf.spheremass, osgHandle_Base);
objects[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(Matrix::translate(0,0,(n==0?-1:1)*conf.axesShift)*pose);
joints[n] = new SliderJoint(objects[Base], pendular[n],
p, Axis((n==0), (n==1), (n==2))*pose);
joints[n]->init(odeHandle, osgHandle, false);
joints[n]->setParam ( dParamStopCFM, 0.1);
joints[n]->setParam ( dParamStopERP, 0.9);
joints[n]->setParam ( dParamCFM, 0.001);
servo[n] =
new SliderServo(dynamic_cast<OneAxisJoint*>(joints[n]),
-conf.diameter*conf.pendularrange,
conf.diameter*conf.pendularrange,
conf.pendularmass*conf.motorpowerfactor,0.1,0.5);
axis[n] = new OSGCylinder(conf.diameter/100, conf.diameter - conf.diameter/100);
axis[n]->init(osgHandleX[n], OSGPrimitive::Low);
}
objects[Pendular1] = pendular[0];
objects[Pendular2] = pendular[1];
objects[Pendular3] = pendular[2];
double sensorrange = conf.irsensorscale * conf.diameter;
RaySensor::rayDrawMode drawMode = conf.drawIRs;
double sensors_inside=0.02;
if(conf.irSensorTempl==0){
conf.irSensorTempl=new IRSensor(conf.irCharacter);
}
irSensorBank.setInitData(odeHandle, osgHandle,
TRANSM(0,0,0) );
irSensorBank.init(0);
if (conf.irAxis1){
for(int i=-1; i<2; i+=2){
RaySensor*
sensor = conf.irSensorTempl->clone();
Matrix R = Matrix::rotate(i*M_PI/2, 1, 0, 0) *
Matrix::translate(0,-i*(conf.diameter/2-sensors_inside),0 );
irSensorBank.registerSensor(sensor, objects[Base], R, sensorrange, drawMode);
}
}
if (conf.irAxis2){
for(int i=-1; i<2; i+=2){
RaySensor*
sensor = conf.irSensorTempl->clone();
Matrix R = Matrix::rotate(i*M_PI/2, 0, 1, 0) *
Matrix::translate(i*(conf.diameter/2-sensors_inside),0,0 );
irSensorBank.registerSensor(sensor, objects[Base], R, sensorrange, drawMode);
}
}
if (conf.irAxis3){
for(int i=-1; i<2; i+=2){
RaySensor*
sensor = conf.irSensorTempl->clone();
Matrix R = Matrix::rotate( i==1 ? 0 : M_PI, 1, 0, 0) *
Matrix::translate(0,0,i*(conf.diameter/2-sensors_inside));
irSensorBank.registerSensor(sensor, objects[Base], R, sensorrange, drawMode);
}
}
if (conf.irRing){
for(double i=0; i<2*M_PI; i+=M_PI/6){
RaySensor*
sensor = conf.irSensorTempl->clone();
Matrix R = Matrix::translate(0,0,conf.diameter/2-sensors_inside) *
Matrix::rotate( i, 0, 1, 0);
irSensorBank.registerSensor(sensor, objects[Base], R, sensorrange, drawMode);
}
}
if (conf.irSide){
for(double i=0; i<2*M_PI; i+=M_PI/2){
RaySensor*
sensor = conf.irSensorTempl->clone();
Matrix R = Matrix::translate(0,0,conf.diameter/2-sensors_inside) *
Matrix::rotate( M_PI/2-M_PI/8, 1, 0, 0) * Matrix::rotate( i, 0, 1, 0);
irSensorBank.registerSensor(sensor, objects[Base], R, sensorrange, drawMode);
sensor = new IRSensor(conf.irCharacter);
irSensorBank.registerSensor(sensor, objects[Base],
R * Matrix::rotate( M_PI, 0, 0, 1),
sensorrange, drawMode);
}
}
FOREACH(list<Sensor*>, conf.sensors, i){
(*i)->init(objects[Base]);
}
created=true;
}
void Sphererobot3Masses::destroy(){
if (created){
for (int i=0; i<servono; i++){
if(servo[i]) delete servo[i];
if(axis[i]) delete axis[i];
}
irSensorBank.clear();
odeHandle.deleteSpace();
}
created=false;
}
void Sphererobot3Masses::notifyOnChange(
const paramkey& key){
for (int i=0; i<servono; i++){
if(servo[i]) servo[i]->setMinMax(-conf.diameter*conf.pendularrange,
conf.diameter*conf.pendularrange);
}
}
}