static void createSensors(::World* world, Joint* jnt, SensorInfoSequence iSensors)
{
int numSensors = iSensors.length();
for(int i=0; i < numSensors; ++i)
{
SensorInfo iSensor = iSensors[i];
//int id = iSensor->id();
int id = iSensor.id;
if(id < 0){
std::cerr << "Warning: sensor ID is not given to sensor " << iSensor.name
<< "of character " << jnt->CharName() << "." << std::endl;
} else {
int sensorType = Sensor::COMMON;
//switch(iSensor.type) {
//case ::FORCE_SENSOR: sensorType = Sensor::FORCE; break;
//case ::RATE_GYRO: sensorType = Sensor::RATE_GYRO; break;
//case ::ACCELERATION_SENSOR: sensorType = Sensor::ACCELERATION; break;
//case ::PRESSURE_SENSOR: sensorType = Sensor::PRESSURE; break;
//case ::PHOTO_INTERRUPTER: sensorType = Sensor::PHOTO_INTERRUPTER; break;
//case ::VISION_SENSOR: sensorType = Sensor::VISION; break;
//case ::TORQUE_SENSOR: sensorType = Sensor::TORQUE; break;
//}
CORBA::String_var type0 = iSensor.type;
string type(type0);
if( type == "Force" ) { sensorType = Sensor::FORCE; } // 6Ž²Í�EZ¥ó¥µ
else if( type == "RateGyro" ) { sensorType = Sensor::RATE_GYRO; } // ¥�E¼¥È¥¸¥ã¥¤¥úÁ»¥ó¥µ
else if( type == "Acceleration" ) { sensorType = Sensor::ACCELERATION; } // ±�E¡¦x¥»¥ó¥µ
else if( type == "Vision" ) { sensorType = Sensor::VISION; } // ¥Ó¥¸¥ç¥ó¥»¥ó¥µ
CORBA::String_var name0 = iSensor.name;
string name(name0);
const DblArray3& p = iSensor.translation;
static fVec3 localPos;
static fMat33 localR;
array_to_vec3(p, localPos);
const DblArray4& rot = iSensor.rotation;
array_to_mat33(rot, localR);
world->addSensor(jnt, sensorType, id, name, localPos, localR);
}
}
}
void createSensors(Link* link, const SensorInfoSequence& sensorInfoSeq, const Matrix33& Rs)
{
int numSensors = sensorInfoSeq.length();
for(int i=0 ; i < numSensors ; ++i ) {
const SensorInfo& sensorInfo = sensorInfoSeq[i];
int id = sensorInfo.id;
if(id < 0) {
std::cerr << "Warning: sensor ID is not given to sensor " << sensorInfo.name
<< "of model " << body->modelName() << "." << std::endl;
} else {
int sensorType = Sensor::COMMON;
CORBA::String_var type0 = sensorInfo.type;
string type(type0);
if(type == "Force") { sensorType = Sensor::FORCE; }
else if(type == "RateGyro") { sensorType = Sensor::RATE_GYRO; }
else if(type == "Acceleration") { sensorType = Sensor::ACCELERATION; }
else if(type == "Vision") { sensorType = Sensor::VISION; }
else if(type == "Range") { sensorType = Sensor::RANGE; }
CORBA::String_var name0 = sensorInfo.name;
string name(name0);
Sensor* sensor = body->createSensor(link, sensorType, id, name);
if(sensor) {
const DblArray3& p = sensorInfo.translation;
sensor->localPos = Rs * Vector3(p[0], p[1], p[2]);
const Vector3 axis(sensorInfo.rotation[0], sensorInfo.rotation[1], sensorInfo.rotation[2]);
const Matrix33 R(rodrigues(axis, sensorInfo.rotation[3]));
sensor->localR = Rs * R;
}
#if 0
if ( sensorType == Sensor::RANGE ) {
RangeSensor *range = dynamic_cast<RangeSensor *>(sensor);
range->scanAngle = sensorInfo.specValues[0];
range->scanStep = sensorInfo.specValues[1];
range->scanRate = sensorInfo.specValues[2];
range->maxDistance = sensorInfo.specValues[3];
}else if (sensorType == Sensor::VISION) {
VisionSensor *vision = dynamic_cast<VisionSensor *>(sensor);
vision->near = sensorInfo.specValues[0];
vision->far = sensorInfo.specValues[1];
vision->fovy = sensorInfo.specValues[2];
vision->width = sensorInfo.specValues[4];
vision->height = sensorInfo.specValues[5];
int npixel = vision->width*vision->height;
switch((int)sensorInfo.specValues[3]){
case Camera::NONE:
vision->imageType = VisionSensor::NONE;
break;
case Camera::COLOR:
vision->imageType = VisionSensor::COLOR;
vision->image.resize(npixel*3);
break;
case Camera::MONO:
vision->imageType = VisionSensor::MONO;
vision->image.resize(npixel);
break;
case Camera::DEPTH:
vision->imageType = VisionSensor::DEPTH;
break;
case Camera::COLOR_DEPTH:
vision->imageType = VisionSensor::COLOR_DEPTH;
vision->image.resize(npixel*3);
break;
case Camera::MONO_DEPTH:
vision->imageType = VisionSensor::MONO_DEPTH;
vision->image.resize(npixel);
break;
}
vision->frameRate = sensorInfo.specValues[6];
}
#endif
}
}
}
