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 selectRandomDirection(AbstractEnvironmentType env, Libutility util, Vector3d preDir) { if (env.env_type == 1) { double randx = (util.getRandDouble() - 0.5) * 2; double randy = (util.getRandDouble() - 0.5) * 2; double randz = (util.getRandDouble() - 0.5) * 2; Vector3d randDir = new Vector3d(randx, randy, randz); randDir = Vector3d.Add(randDir, preDir); Double leng = randDir.Length; Double factor = _cur_speed / leng; orientation = Vector3d.Multiply(factor, randDir); return; } else if (env.env_type == 2) { double randx = (util.getRandDouble() - 0.5) * 2; double randy = (util.getRandDouble() - 0.5) * 2; Vector3d randDir = new Vector3d(randx, randy, 0); randDir = Vector3d.Add(randDir, preDir); randDir.Unitize(); uv_orientation = Vector3d.Multiply(_cur_speed, randDir); orientation = env.getOrientationFromUv(Location, uv_orientation); return; } }
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(); }