public void doSensorBehaviorsInBoxenv(AbstractEnvironmentType env, Libutility util)
{
this.doDeathTest(env);
//orientation = env.projectOrientationToEnv(Location, orientation);
int det_count = PhysaSetting.DetectDirRSubd * PhysaSetting.DetectDirPhySubd + 1;
int max_item = 0;
float max_item_phy = 0;
float max_item_theta = 0;
sensor_data = new float[det_count];
//List<trailInfo> infos = new List<trailInfo>();
//infos.Add(env.getOffsetTrailValue(curx, cury, curz, orientation, 0, 0, _sensor_offset, util));
//float maxtrail = 0;
//Point3d tgtPos = new Point3d();
sensor_data[0] = env.getOffsetTrailValue(curx, cury, curz, orientation, 0, 0, PhysaSetting._sense_offset, util);
int count_cur = 1;
for (int i = 0; i < PhysaSetting.DetectDirRSubd; i++)
{
for (int j = 1; j <= PhysaSetting.DetectDirPhySubd; j++)
{
sensor_data[count_cur] = env.getOffsetTrailValue(curx, cury, curz, orientation, j * PhysaSetting._sensor_phy_step_angle, i * PhysaSetting._sensor_theta_step_angle, PhysaSetting._sense_offset, util);
//infos.Add(env.getOffsetTrailValue(curx, cury, curz, orientation, j * _sensor_phy_step_angle, i * _sensor_theta_step_angle, _sensor_offset, util));
if (sensor_data[count_cur] > sensor_data[max_item])
{
max_item = count_cur;
max_item_phy = j * PhysaSetting._sensor_phy_step_angle;
max_item_theta = i * PhysaSetting._sensor_theta_step_angle;
}
count_cur++;
}
}
/*foreach(trailInfo inf in infos)
* {
* if(inf.trailValue > maxtrail)
* {
* maxtrail = inf.trailValue;
* tgtPos = inf.targetPos;
* }
* }
* Vector3d newOri = Point3d.Subtract(tgtPos, new Point3d(curx, cury, curz));
* double curLength = newOri.Length;
* double scaleFactor = _cur_speed / curLength;
* orientation = Vector3d.Multiply(scaleFactor, newOri);
* //orientation = newOri;
*/
rotate(max_item_phy * PhysaSetting._rotate_angle / PhysaSetting._sense_angle, max_item_theta);
guideOrientation();
}
public void doSensorBehaviorsInSrfenv(AbstractEnvironmentType env, Libutility util)
{
this.doDeathTest(env);
int det_count = PhysaSetting.DetectDirPhySubd * 2 + 1;
int max_item = 0;
float max_item_phy = -PhysaSetting._sense_angle;
sensor_data = new float[det_count];
//List<trailInfo> infos = new List<trailInfo>();
//infos.Add(env.getOffsetTrailValue(curx, cury, curz, orientation, 0, 0, _sensor_offset, util));
//float maxtrail = 0;
//Point3d tgtPos = new Point3d();
//sensor_data[0] = env.getOffsetTrailValue(curx, cury, curz, orientation, 0, 0, PhysaSetting._sense_offset, util);
int count_cur = 0;
for (int j = -PhysaSetting.DetectDirPhySubd; j <= PhysaSetting.DetectDirPhySubd; j++)
{
sensor_data[count_cur] = env.getOffsetTrailValue(curx, cury, curz, uv_orientation, j * PhysaSetting._sensor_phy_step_angle, 0, PhysaSetting._sense_offset, util);
//infos.Add(env.getOffsetTrailValue(curx, cury, curz, orientation, j * _sensor_phy_step_angle, i * _sensor_theta_step_angle, _sensor_offset, util));
if (sensor_data[count_cur] > sensor_data[max_item])
{
max_item = count_cur;
max_item_phy = j * PhysaSetting._sensor_phy_step_angle;
}
count_cur++;
}
//this.tempValue = env.getOffsetTrailValue(curx, cury, curz, uv_orientation, 0, 0, PhysaSetting._sense_offset, util);
//max_item_phy = 90;
rotate2D(max_item_phy * PhysaSetting._rotate_angle / PhysaSetting._sense_angle);
orientation = env.getOrientationFromUv(Location, uv_orientation);
guideOrientation();
}
public void doSensorBehaviorsInSrfenv(AbstractEnvironmentType env, Libutility util)
{
this.doDeathTest(env);
int det_count = PhysaSetting.DetectDirPhySubd * 2 + 1;
int max_item = 0;
float max_item_phy = -PhysaSetting._sense_angle;
sensor_data = new float[det_count];
//List<trailInfo> infos = new List<trailInfo>();
//infos.Add(env.getOffsetTrailValue(curx, cury, curz, orientation, 0, 0, _sensor_offset, util));
//float maxtrail = 0;
//Point3d tgtPos = new Point3d();
//sensor_data[0] = env.getOffsetTrailValue(curx, cury, curz, orientation, 0, 0, PhysaSetting._sense_offset, util);
int count_cur = 0;
for (int j = -PhysaSetting.DetectDirPhySubd; j <= PhysaSetting.DetectDirPhySubd; j++)
{
sensor_data[count_cur] = env.getOffsetTrailValue(curx, cury, curz, uv_orientation, j * PhysaSetting._sensor_phy_step_angle,0, PhysaSetting._sense_offset, util);
//infos.Add(env.getOffsetTrailValue(curx, cury, curz, orientation, j * _sensor_phy_step_angle, i * _sensor_theta_step_angle, _sensor_offset, util));
if (sensor_data[count_cur] > sensor_data[max_item])
{
max_item = count_cur;
max_item_phy = j * PhysaSetting._sensor_phy_step_angle;
}
count_cur++;
}
//this.tempValue = env.getOffsetTrailValue(curx, cury, curz, uv_orientation, 0, 0, PhysaSetting._sense_offset, util);
//max_item_phy = 90;
rotate2D(max_item_phy * PhysaSetting._rotate_angle / PhysaSetting._sense_angle);
orientation = env.getOrientationFromUv(Location, uv_orientation);
guideOrientation();
}
public void doSensorBehaviorsInBoxenv(AbstractEnvironmentType env, Libutility util)
{
this.doDeathTest(env);
//orientation = env.projectOrientationToEnv(Location, orientation);
int det_count = PhysaSetting.DetectDirRSubd * PhysaSetting.DetectDirPhySubd + 1;
int max_item = 0;
float max_item_phy = 0;
float max_item_theta = 0;
sensor_data = new float[det_count];
//List<trailInfo> infos = new List<trailInfo>();
//infos.Add(env.getOffsetTrailValue(curx, cury, curz, orientation, 0, 0, _sensor_offset, util));
//float maxtrail = 0;
//Point3d tgtPos = new Point3d();
sensor_data[0] = env.getOffsetTrailValue(curx, cury, curz, orientation, 0, 0, PhysaSetting._sense_offset, util);
int count_cur = 1;
for (int i = 0; i < PhysaSetting.DetectDirRSubd; i++)
{
for (int j = 1; j <= PhysaSetting.DetectDirPhySubd; j++)
{
sensor_data[count_cur] = env.getOffsetTrailValue(curx, cury, curz, orientation, j * PhysaSetting._sensor_phy_step_angle, i * PhysaSetting._sensor_theta_step_angle, PhysaSetting._sense_offset, util);
//infos.Add(env.getOffsetTrailValue(curx, cury, curz, orientation, j * _sensor_phy_step_angle, i * _sensor_theta_step_angle, _sensor_offset, util));
if (sensor_data[count_cur] > sensor_data[max_item])
{
max_item = count_cur;
max_item_phy = j * PhysaSetting._sensor_phy_step_angle;
max_item_theta = i * PhysaSetting._sensor_theta_step_angle;
}
count_cur++;
}
}
/*foreach(trailInfo inf in infos)
{
if(inf.trailValue > maxtrail)
{
maxtrail = inf.trailValue;
tgtPos = inf.targetPos;
}
}
Vector3d newOri = Point3d.Subtract(tgtPos, new Point3d(curx, cury, curz));
double curLength = newOri.Length;
double scaleFactor = _cur_speed / curLength;
orientation = Vector3d.Multiply(scaleFactor, newOri);
//orientation = newOri;
*/
rotate(max_item_phy * PhysaSetting._rotate_angle / PhysaSetting._sense_angle, max_item_theta);
guideOrientation();
}