int loop_hook(t_env *e)
{
t_rgb c;
int x;
if (e->img.img != NULL)
{
//Changed form mlx_destroy_image to this.
mlx_destroy_image(e->mlx, e->img.img);
e->img.img = NULL;
}
e->img.img = mlx_new_image(e->mlx, WIN_WIDTH, WIN_HEIGH);
x = 0;
while (x < WIN_WIDTH)
{
init_ray(e, x);
ray_dir(e);
perform_dda(e);
compute(e);
colors(e, &c);
draw_line(x, e, &c);
x++;
}
get_time_frame(e);
move(e);
mlx_put_image_to_window(e->mlx, e->win, e->img.img, 0, 0);
return (0);
}
int simulateMeasurement(Ray measurementAction, RayTracer &rayt,
ros::ServiceClient pfilterAdd, double noiseStdDev)
{
std::random_device rd;
std::normal_distribution<double> randn(0.0, noiseStdDev);
tf::Point start = measurementAction.start;
tf::Point end = measurementAction.end;
tf::Point intersection;
geometry_msgs::Point obs;
geometry_msgs::Point dir;
double distToPart;
if(!rayt.traceRay(measurementAction, distToPart)){
ROS_INFO("NO INTERSECTION, Skipping");
return -1;
}
intersection = start + (end-start).normalize() * (distToPart);
std::cout << "Intersection at: " << intersection.getX() << " " << intersection.getY() << " " << intersection.getZ() << std::endl;
tf::Point ray_dir(end.x()-start.x(),end.y()-start.y(),end.z()-start.z());
ray_dir = ray_dir.normalize();
obs.x=intersection.getX() + randn(rd);
obs.y=intersection.getY() + randn(rd);
obs.z=intersection.getZ() + randn(rd);
dir.x=ray_dir.x();
dir.y=ray_dir.y();
dir.z=ray_dir.z();
particle_filter::AddObservation pfilter_obs;
pfilter_obs.request.p = obs;
pfilter_obs.request.dir = dir;
if(!pfilterAdd.call(pfilter_obs)){
ROS_INFO("Failed to call add observation");
}
return 1;
}
/*
* Intersects all the colliders in the scene with the input ray
* and returns the list of collisions.
*
* This function is not thread-safe because it relies on a static
* array of colliders which could be updated by a different thread.
*/
void Picker::pickScene(Scene* scene, std::vector<ColliderData>& picklist, Transform* t,
float ox, float oy, float oz, float dx, float dy, float dz) {
glm::vec3 ray_start(ox, oy, oz);
glm::vec3 ray_dir(dx, dy, dz);
const std::vector<Component*>& colliders = scene->lockColliders();
const glm::mat4& model_matrix = t->getModelMatrix();
Collider::transformRay(model_matrix, ray_start, ray_dir);
for (auto it = colliders.begin(); it != colliders.end(); ++it) {
Collider* collider = reinterpret_cast<Collider*>(*it);
SceneObject* owner = collider->owner_object();
if (collider->enabled() && (owner != NULL) && owner->enabled()) {
ColliderData data = collider->isHit(ray_start, ray_dir);
if ((collider->pick_distance() > 0) && (collider->pick_distance() < data.Distance)) {
data.IsHit = false;
}
if (data.IsHit) {
picklist.push_back(data);
}
}
}
std::sort(picklist.begin(), picklist.end(), compareColliderData);
scene->unlockColliders();
}
// IRQ1 interrupt handler sets keys buffer for this function to read
void handle_key(uint8_t key) {
hit info;
// If the highest bit is not set, this key has not changed
if (!(keys[key] & 0x80)) {
return;
}
bool down = keys[key] & 1;
// Mark key state as read
keys[key] &= 1;
switch (key) {
// View
case KEY_UP:
dPitch += down ? 1.0f : -1.0f;
break;
case KEY_DOWN:
dPitch += down ? -1.0f : 1.0f;
break;
case KEY_LEFT:
dYaw += down ? 1.0f : -1.0f;
break;
case KEY_RIGHT:
dYaw += down ? -1.0f : 1.0f;
break;
case KEY_SPACE:
if (down) {
playerPos.y += 0.1f;
velocity.y += 8.0f;
}
break;
// Check if a block was hit and place a new block next to it
case KEY_Q:
if (!down) {
raytrace(playerPos, ray_dir(160, 100), &info);
if (info.hit) {
int bx = info.x + info.nx;
int by = info.y + info.ny;
int bz = info.z + info.nz;
if (IN_WORLD(bx, by, bz)) {
set_block(bx, by, bz, BLOCK_DIRT);
}
}
}
break;
// Check if a block was hit and remove it
case KEY_E:
if (!down) {
raytrace(playerPos, ray_dir(160, 100), &info);
if (info.hit) {
set_block(info.x, info.y, info.z, BLOCK_AIR);
}
}
break;
case KEY_ESC:
init_world();
break;
}
// Make sure view look speed doesn't add up with low framerates
if (dPitch != 0.0f) dPitch /= absf(dPitch);
if (dYaw != 0.0f) dYaw /= absf(dYaw);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "updating_particles");
ros::NodeHandle n;
PlotRayUtils plt;
RayTracer rayt;
std::random_device rd;
std::normal_distribution<double> randn(0.0,0.000001);
ROS_INFO("Running...");
ros::Publisher pub_init =
n.advertise<particle_filter::PFilterInit>("/particle_filter_init", 5);
ros::ServiceClient srv_add =
n.serviceClient<particle_filter::AddObservation>("/particle_filter_add");
ros::Duration(1).sleep();
// pub_init.publish(getInitialPoints(plt));
geometry_msgs::Point obs;
geometry_msgs::Point dir;
double radius = 0.00;
int i = 0;
//for(int i=0; i<20; i++){
while (i < NUM_TOUCHES) {
// ros::Duration(1).sleep();
//tf::Point start(0.95,0,-0.15);
//tf::Point end(0.95,2,-0.15);
tf::Point start, end;
// randomSelection(plt, rayt, start, end);
fixedSelection(plt, rayt, start, end);
Ray measurement(start, end);
double distToPart;
if(!rayt.traceRay(measurement, distToPart)){
ROS_INFO("NO INTERSECTION, Skipping");
continue;
}
tf::Point intersection(start.getX(), start.getY(), start.getZ());
intersection = intersection + (end-start).normalize() * (distToPart - radius);
std::cout << "Intersection at: " << intersection.getX() << " " << intersection.getY() << " " << intersection.getZ() << std::endl;
tf::Point ray_dir(end.x()-start.x(),end.y()-start.y(),end.z()-start.z());
ray_dir = ray_dir.normalize();
obs.x=intersection.getX() + randn(rd);
obs.y=intersection.getY() + randn(rd);
obs.z=intersection.getZ() + randn(rd);
dir.x=ray_dir.x();
dir.y=ray_dir.y();
dir.z=ray_dir.z();
// obs.x=intersection.getX();
// obs.y=intersection.getY();
// obs.z=intersection.getZ();
// pub_add.publish(obs);
// plt.plotCylinder(start, end, 0.01, 0.002, true);
plt.plotRay(Ray(start, end));
// ros::Duration(1).sleep();
particle_filter::AddObservation pfilter_obs;
pfilter_obs.request.p = obs;
pfilter_obs.request.dir = dir;
if(!srv_add.call(pfilter_obs)){
ROS_INFO("Failed to call add observation");
}
ros::spinOnce();
while(!rayt.particleHandler.newParticles){
ROS_INFO_THROTTLE(10, "Waiting for new particles...");
ros::spinOnce();
ros::Duration(.1).sleep();
}
i ++;
}
// std::ofstream myfile;
// myfile.open("/home/shiyuan/Documents/ros_marsarm/diff.csv", std::ios::out|std::ios::app);
// myfile << "\n";
// myfile.close();
// myfile.open("/home/shiyuan/Documents/ros_marsarm/time.csv", std::ios::out|std::ios::app);
// myfile << "\n";
// myfile.close();
// myfile.open("/home/shiyuan/Documents/ros_marsarm/diff_trans.csv", std::ios::out|std::ios::app);
// myfile << "\n";
// myfile.close();
// myfile.open("/home/shiyuan/Documents/ros_marsarm/diff_rot.csv", std::ios::out|std::ios::app);
// myfile << "\n";
// myfile.close();
// myfile.open("/home/shiyuan/Documents/ros_marsarm/workspace_max.csv", std::ios::out|std::ios::app);
// myfile << "\n";
// myfile.close();
// myfile.open("/home/shiyuan/Documents/ros_marsarm/workspace_min.csv", std::ios::out|std::ios::app);
// myfile << "\n";
// myfile.close();
ROS_INFO("Finished all action");
}
