std::vector<double> Align::getAngVel(unsigned int ticks, unsigned int delta_ticks) {
double steering;
double rotation, rotationSize, targetRotation;
Triple direction;
rotation = mapToRange(mapToRange(target->ang) - mapToRange(character->ang));
rotationSize = abs(rotation);
if (rotationSize < targetRadius) {
#ifdef DEBUG_ALIGN
std::cout << "Align " << static_cast<void *>(this) << ": dentro de targetRadius" << std::endl;
#endif
steering = target->vrot - character->vrot;
if (abs(steering) > maxAngularVelocity) {
steering = maxAngularVelocity;
}
return std::vector<double>(1, steering);
}
targetRotation = maxAngularVelocity;
if (rotationSize < slowRadius){
targetRotation *= rotationSize / slowRadius;
#ifdef DEBUG_ALIGN
std::cout << "Align " << static_cast<void *>(this) << ": entre targetRadius y slowRadius; targetRotation = " << targetRotation << "; rotation = " << rotation << std::endl;
#endif
}
#ifdef DEBUG_ALIGN
else {
std::cout << "Align " << static_cast<void *>(this) << ": fuera de slowRadius; targetRotation = " << targetRotation << "; rotation = " << rotation << std::endl;
}
#endif
//targetRotation *= map_atan(20000 * target->vrot);
steering = targetRotation * (rotation > 0 ? -1 : 1);
return std::vector<double>(1, steering);
}
//-----------------------------------------------------------------------------------------
QPixmap* HeightImageEditorCodec::onBeforeDisplay(Ogre::DataStreamPtr stream)
{
double file_len = stream->size() / 4;
unsigned int map_size = sqrt(file_len);
mBuffer = new unsigned char[map_size * map_size * sizeof(float)];
float *buf = (float *)mBuffer;
stream->read(buf, map_size * map_size * 4);
float max_h = -1000000.0f;
float min_h = 1000000.0f;
for(unsigned int i = 0;i < map_size * map_size;i++)
{
if(buf[i] > max_h)
max_h = buf[i];
if(buf[i] < min_h)
min_h = buf[i];
}
float diff = max_h - min_h;
if(diff > 0.0f)
{
unsigned char *colbuf = (unsigned char *)buf;
int pos = 0;
for(unsigned int i = 0;i < map_size * map_size;i++)
{
float tval = mapToRange(buf[i], min_h, max_h, 60, 360);
HSVtoARGB(&colbuf[pos], tval, 1.0f, 1.0f);
pos += 4;
}
}
else
memset(buf, 0xFF, map_size * map_size * 4);
mImage = QImage((unsigned char *)buf, map_size, map_size, QImage::Format_ARGB32);
QPixmap *pixmap = new QPixmap(QPixmap::fromImage(mImage));
return pixmap;
}
std::vector<double> Face::getAngVelIncr(unsigned int ticks, unsigned int delta_ticks) {
double steering;
double rotation, rotationSize, targetRotation;
Triple direction;
Triple cp, tp;
std::tie(cp, tp) = points(this->character, this->target);
direction = tp - cp;
rotation = mapToRange(atan2(direction.y, direction.x) - character->ang);
//if (rotation > M_PI) rotation -= 2 * M_PI;
rotationSize = abs(rotation);
if (rotationSize < targetRadius) {
#ifdef DEBUG_FACE
std::cout << "Face " << static_cast<void *>(this) << ": dentro de targetRadius" << std::endl;
#endif
steering = target->vrot - character->vrot;
if (abs(steering) > maxAngularVelocity) {
steering = maxAngularVelocity;
}
return std::vector<double>(1, steering);
}
targetRotation = maxAngularVelocity; // - target->vrot;
if (rotationSize < slowRadius) {
#ifdef DEBUG_FACE
std::cout << "Face " << static_cast<void *>(this) << ": entre targetRadius y slowRadius" << std::endl;
#endif
targetRotation *= rotationSize / slowRadius;
}
#ifdef DEBUG_FACE
else {
std::cout << "Face " << static_cast<void *>(this) << ": fuera de slowRadius" << std::endl;
}
#endif
//if (targetRotation < 0) targetRotation = 0;
steering = targetRotation * (rotation > 0 ? -1 : 1);
return std::vector<double>(1, steering);
}
}
// Never reached because the threads run forever
terminateJoystick(&joystick);
closeSerial(&serialPort);
return 0;
}
void *joystickUpdater(__attribute__ ((unused)) void *argument)
{
while (1)
{
updateJoystick(&joystick);
roll = mapToRange(-1 * joystick.axis[0]);
pitch = mapToRange(-1 * joystick.axis[1]);
yaw = mapToRange(joystick.axis[2]);
throttle = mapToRange(-1 * joystick.axis[3]);
button1 = joystick.button[0];
button2 = joystick.button[1];
button3 = joystick.button[2];
button4 = joystick.button[3];
button5 = joystick.button[4];
button6 = joystick.button[5];
usleep(1000000 / FREQ_JOYSTICK);
}
pthread_exit(0);
}
