float get_expected_distance(struct robot *robot)
{
struct vect local_look, look, offset, world_offset, offset_cam, cam_in_world, floor_spot;
look.x = 0;
look.y = 10;
look.z = 0;
offset.x = 0;
offset.y = 0;
offset.z = 150;
cam_to_world(robot, &local_look, &look);
cam_to_world(robot, &world_offset, &offset);
local_to_world(robot, &cam_in_world, &camera_center);
offset_cam.x = cam_in_world.x + world_offset.x;
offset_cam.y = cam_in_world.y + world_offset.y;
offset_cam.z = cam_in_world.z + world_offset.z;
find_floor_intersection(&floor_spot, &offset_cam, &local_look);
float xx = (floor_spot.x - cam_in_world.x) * (floor_spot.x - cam_in_world.x);
float yy = (floor_spot.y - cam_in_world.y) * (floor_spot.y - cam_in_world.y);
float zz = (floor_spot.z - cam_in_world.z) * (floor_spot.z - cam_in_world.z);
return sqrtf(xx + yy +zz) - 35;
}
void cam_to_world(struct robot *robot, struct vect *out, struct vect *in)
{
struct vect temp;
cam_to_local(robot, &temp, in);
local_to_world(robot, out, &temp);
out->x -= robot->position2.position.x;
out->y -= robot->position2.position.y;
}
Spectrum BSDF::evaluate_sample_f(glm::vec3 outgoing_w, glm::vec3* incident_w, float u1, float u2, float* pdf) const {
assert(u1 <= 1.0);
if (_brdf_count < 1) {
return Spectrum(0, 0, 0);
}
int brdf_num = glm::round(u1 * (_brdf_count-1));
BRDF* brdf = _brdfs[brdf_num];
glm::vec3 local_outgoing = world_to_local(outgoing_w);
glm::vec3 local_incident;
Spectrum f = brdf->evaluate_sample_f(local_outgoing, &local_incident, u1, u2, pdf) * _brdf_scales[brdf_num];
*incident_w = local_to_world(local_incident);
return f;
}
void screen_to_world_vect(struct robot *robot, struct vect *out,
float x, float y)
{
struct vect local_look, look, cam_in_world, floor_spot;
look.x = 200 * x;
look.y = 1000;
look.z = 200 * y;
cam_to_world(robot, &local_look, &look);
local_to_world(robot, &cam_in_world, &camera_center);
// fprintf(stderr, "%f, %f, %f\n", local_look.x, local_look.y, local_look.z);
find_floor_intersection(&floor_spot, &cam_in_world, &local_look);
*out = floor_spot;
// fprintf(stderr, "%f, %f, %f\n", out->x, out->y, out->z);
}
void pointer(struct robot *robot)
{
struct vect point;
point.x = (cosf(robot->rotational_target + (robot->rotational_velocity * 0)) * 1000) + robot->position2.position.x;
point.y = (sinf(robot->rotational_target + (robot->rotational_velocity * 0)) * 1000) + robot->position2.position.y;
point.z = -40;
//fprintf(stderr, "%f, %f, %f\n\n", robot->turret.target.x, robot->turret.target.y, robot->turret.target.z);
point_at_world(robot, &robot->turret.target);
struct vect cam_in_world;
local_to_world(robot, &cam_in_world, &camera_center);
// fprintf(stderr, "%f, %f, %f\n", cam_in_world.x, cam_in_world.y, cam_in_world.z);
// fprintf(stderr, "%f, %f\n", robot->position2.position.x, robot->position2.position.y);
struct vect d;
difference(&d, &cam_in_world, &point);
float xx = d.x * d.x;
float yy = d.y * d.y;
float zz = d.z * d.z;
float predicted_distance = sqrtf(xx + yy + zz) - 40;
//fprintf(stderr, "%f, %f\n", predicted_distance, robot->range);
if (predicted_distance < robot->range)
{
gpioWrite(14, 0);
}
else
{
gpioWrite(14, 0);
}
struct vect output;
get_up_vector(robot, &output);
// printf("%f, %f, %f\n", output.x, output.y, output.z);
struct vect cam_up, up, look, cam_forward, product;
up.x = 0;
up.y = 0;
up.z = 1.0;
cam_forward.x = 0;
cam_forward.y = -1;
cam_forward.z = 0;
rotate_vector(&cam_up, &up, &robot->turret.turret_rotation);
rotate_vector(&look, &cam_forward, &robot->turret.turret_rotation);
robot->turret.roll = acosf(dot(&cam_up, &output));
cross(&cam_up, &output, &product);
robot->turret.roll = -asinf(product.x / look.x);
if (robot->turret.pitch > robot->turret.pitch_guess)
{
robot->turret.pitch_guess += 0.000000174532925;
if (robot->turret.pitch_guess > robot->turret.pitch)
{
robot->turret.pitch_guess = robot->turret.pitch;
}
}
if (robot->turret.pitch < robot->turret.pitch_guess)
{
robot->turret.pitch_guess -= 0.000000174532925;
if (robot->turret.pitch_guess < robot->turret.pitch)
{
robot->turret.pitch_guess = robot->turret.pitch;
}
}
if (robot->turret.yaw > robot->turret.yaw_guess)
{
robot->turret.yaw_guess += 0.000000174532925;
if (robot->turret.yaw_guess > robot->turret.yaw)
{
robot->turret.yaw_guess = robot->turret.yaw;
}
}
if (robot->turret.yaw < robot->turret.yaw_guess)
{
robot->turret.yaw_guess -= 0.000000174532925;
if (robot->turret.yaw_guess < robot->turret.yaw)
{
robot->turret.yaw_guess = robot->turret.yaw;
}
}
//printf("%f, %f, %f\n", local.x, local.y, local.z);
//fprintf(stderr, "%f\n", robot->turret.roll);
}
