QVector3D EnvRoomTemplate::ClipPlayerTrajectory(const QVector3D & pos, const QVector3D & vel) const
{
QVector3D new_vel;
bool x_collide = false;
bool z_collide = false;
QPointF move(pos.x() + vel.x(), pos.z() + vel.z());
QPointF move_x(pos.x() + vel.x(), pos.z());
QPointF move_z(pos.x(), pos.z() + vel.z());
for (int i=0; i<colliders.size(); ++i) {
if (colliders[i].contains(move)) {
if (colliders[i].contains(move_x)) {
x_collide = true;
}
if (colliders[i].contains(move_z)) {
z_collide = true;
}
}
}
if (!x_collide && z_collide) {
new_vel = QVector3D(vel.x(), 0, 0);
}
else if (!z_collide && x_collide) {
new_vel = QVector3D(0, 0, vel.z());
}
else if (x_collide && z_collide) {
new_vel = QVector3D(0, 0, 0);
}
else {
new_vel = vel;
}
move = QPointF(pos.x() + new_vel.x(), pos.z() + new_vel.z());
for (int i=0; i<circ_colliders.size(); ++i) {
QVector3D cent_dir = QVector3D(circ_colliders[i].pos.x() - move.x(), 0, circ_colliders[i].pos.y() - move.y());
const float cent_dist = cent_dir.length();
if ((circ_colliders[i].stay_inside && cent_dist > circ_colliders[i].rad) ||
(!circ_colliders[i].stay_inside && cent_dist < circ_colliders[i].rad)) {
new_vel += cent_dir.normalized() * (cent_dist - circ_colliders[i].rad);
}
}
/*
qDebug() << "the mountpoints:";
for (int angle=0; angle<=360; angle+=18) {
if (angle % 36 == 0) {
qDebug() << sinf(float(angle)* MathUtil::_PI_OVER_180) * 20.0f << "0" << cosf(float(angle)* MathUtil::_PI_OVER_180) * 20.0f
<< -sinf(float(angle)* MathUtil::_PI_OVER_180) << "0" << -cosf(float(angle)* MathUtil::_PI_OVER_180);
}
else {
qDebug() << sinf(float(angle)* MathUtil::_PI_OVER_180) * 12.1f << "0" << cosf(float(angle)* MathUtil::_PI_OVER_180) * 12.1f
<< sinf(float(angle)* MathUtil::_PI_OVER_180) << "0" << cosf(float(angle)* MathUtil::_PI_OVER_180);
}
}
*/
return new_vel;
}
void check_controller(rov_context_t *context)
{
JoystickEvent event;
bool result = context->joystick->sample(&event);
if(result && !event.isInitialState())
{
if(event.isAxis())
{
int value = event.value;
switch(event.number)
{
case 0: // The x axis
{
x = value;
move_xy(context, x, y);
} break;
case 1: // The y axis
{
y = value;
move_xy(context, x, y);
} break;
case 2: // The twist axis
{
rotate_z(context, value);
} break;
case 3: // The kull axis
{
move_z(context, value);
} break;
case 4: // Top x-axis
{
top_x = value;
balance(context, top_x, top_y);
} break;
case 5: // Top y-axis
{
top_y = value;
balance(context, top_x, top_y);
} break;
}
}
else if(event.isButton())
{
if (event.value)
{
switch(event.number)
{
case 0:
{
rov_interface::control_gripper(context, 20);
} break;
case 1:
{
rov_interface::control_gripper(context, 150);
} break;
case 2:
{
rov_interface::switch_camera(context, 3);
} break;
case 3:
{
rov_interface::switch_camera(context, 4);
} break;
case 4:
{
rov_interface::switch_camera(context, 8);
} break;
case 8:
{
valve_opener_pwm -= 16;
} break;
case 9:
{
valve_opener_pwm += 16;
} break;
}
}
if (event.number == 10)
{
rov_interface::set_motor(context, VALVE_OPENER, event.value, false, valve_opener_pwm);
}
else if (event.number == 11)
{
rov_interface::set_motor(context, VALVE_OPENER, false, event.value, valve_opener_pwm);
}
}
}
}
