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); }