00001
00002
00003
00004 #ifndef CAJUN_SIM_WORLD_OBJECT_H
00005 #define CAJUN_SIM_WORLD_OBJECT_H
00006
00007 #include <ode/ode.h>
00008 #include "sim_util.H"
00009 #include "object_info.H"
00010 #include "rndf_tool.H"
00011 #include "access_dq.H"
00012
00013 #define MAX_OBSTACLE_JOINTS 57*7 //FIXME: A bodypart might touch
00014 #define MAX_BODIES 5 //at max 4 obstacles
00015
00016 namespace cajun
00017 {
00018 class sim_world_object_t
00019 {
00020 public:
00021 sim_world_object_t ();
00022 sim_world_object_t (dSpaceID *world_space, object_info_t *object_data,
00023 rndf_tool_t *rndf_tool, access_data_t *m_access_data);
00024 virtual ~sim_world_object_t ();
00025 virtual bool update (float) = 0;
00026 virtual void get_data (vector<obstacle_data_t> *&object_info,
00027 vector <obstacle_points_data_t> *&object_points_) = 0;
00028
00029 void set_robot_rotation (int body_num, dReal const rotation_matrix[16]);
00030 void get_robot_position (int part, double p[3]);
00031 void get_robot_rotation (int part, double rot[12]);
00032 void get_acceleration (int body_num_, double &x_accel_,
00033 double &y_accel_, double &z_accel_);
00034 void get_angular_rate (int body_num_, double &x_vel_,
00035 double &y_vel_, double &z_vel_);
00036
00037 void get_orientation (int body_num, double &orientation_x, double &orientation_y,
00038 double &orientation_z);
00039 void get_rotation_matrix (int body_num, double orientation_matrix[3][4],
00040 int direction);
00041 void get_bot_position (double &x, double &y, double &z);
00042 void set_geom_orientation (dGeomID *object, double azimuth,
00043 double elivation, double tilt);
00044 void set_object_enable (bool enabled_);
00045 inline void collided () { m_collided = true; };
00046 inline int obj_id () { return m_object_id; };
00047
00048 dBodyID m_carb[MAX_BODIES];
00049 dGeomID m_carg[MAX_BODIES];
00050 object_info_t m_object_data;
00051 rndf_tool_t *m_rndf_tool;
00052 access_data_t *m_access_data;
00053
00054 double m_x_far;
00055 double m_x_close;
00056 double m_y_far;
00057 double m_y_close;
00058 double m_x_far_neg;
00059 double m_x_close_neg;
00060 double m_y_far_neg;
00061 double m_y_close_neg;
00062
00063 protected:
00064 timer_t *m_timer;
00065 vector<obstacle_data_t> current_object_data;
00066 vector<obstacle_points_data_t> current_object_points;
00067 int m_body_part_counter;
00068 double m_currentTime;
00069 dReal *prev_vel;
00070
00071 float x_offset, y_offset;
00072
00073 obstacle_data_t::obstacle_shape_t m_shape;
00074 double m_heading;
00075 double m_attitude;
00076 double m_bank;
00077 double m_x_size;
00078 double m_y_size;
00079 double m_z_size;
00080 int m_object_id;
00081
00082 dGeomID m_geom_transform[MAX_BODIES];
00083 dJointID m_hing[MAX_BODIES];
00084 dSpaceID m_local_space;
00085 dJointGroupID m_hgrp;
00086 dJointID m_obstacle_joint[MAX_OBSTACLE_JOINTS];
00087
00088 bool m_collided;
00089 private:
00090 orientation_t m_orientation;
00091 };
00092 }
00093 #endif