public void InitLidar(LidarParams lp)
{
lidar = new Lidar(lp, body);
}
public Car(Physics ph,CarParams p,Pose pose)
{
this.p = p;
car_angle0 = Helper.GetRelAngle(new Vec2(1, 0), forwardVec);
body = ph.CreateBox(new Pose { xc = pose.xc, yc = pose.yc }, new Box2DX.Common.Vec2 { X = p.w, Y = p.h }, new BodyBehaviour { isDynamic = true, k = 0.98f * p.mass });
bodyFW1 = ph.CreateBox(new Pose { xc = pose.xc, yc = pose.yc + p.h_base / 2 }, new Box2DX.Common.Vec2 { X = p.w / 10, Y = p.h / 10 }, new BodyBehaviour { isDynamic = true, k = 0.01f * p.mass });
bodyBW1 = ph.CreateBox(new Pose { xc = pose.xc, yc = pose.yc + -p.h_base / 2 }, new Box2DX.Common.Vec2 { X = p.w / 10, Y = p.h / 10 }, new BodyBehaviour { isDynamic = true, k = 0.01f * p.mass });
var jFW1def = new RevoluteJointDef();
jFW1def.Initialize(body, bodyFW1, bodyFW1.GetWorldCenter());
jFW1def.EnableMotor = true;
jFW1def.MaxMotorTorque = 1000;
jFW1def.EnableLimit = true;
jFW1def.LowerAngle = -p.max_steer_angle * Helper.angle_to_rad;
jFW1def.UpperAngle = p.max_steer_angle * Helper.angle_to_rad;
jFW1 = (RevoluteJoint)ph.world.CreateJoint(jFW1def);
var jBW1def = new PrismaticJointDef();
jBW1def.Initialize(body, bodyBW1, bodyBW1.GetWorldCenter(), new Vec2(1, 0));
jBW1def.EnableLimit = true;
jBW1def.UpperTranslation = jBW1def.LowerTranslation = 0;
jBW1 = (PrismaticJoint)ph.world.CreateJoint(jBW1def);
//LidarParams lp = new LidarParams() { dir_deg = 0, d0 = 2.2f, dist = 20, fov_deg = 60, x0 = 0, y0 = 0, num_rays = 20 };
var p1 = new LidarParams();
p1.InitDefault();
p1.d0 = p.h / 2 + 0.2f;
InitLidar(p1);
body.SetAngle(pose.angle_rad);
//rcs = new RCS(this);
}
public Lidar(LidarParams p_, Car parent_car_)
{
p = p_;
parent_car = parent_car_;
data = new float[p.num_rays];
}