Branch data Line data Source code
1 : : // Copyright 2022 Garena Online Private Limited
2 : : //
3 : : // Licensed under the Apache License, Version 2.0 (the "License");
4 : : // you may not use this file except in compliance with the License.
5 : : // You may obtain a copy of the License at
6 : : //
7 : : // http://www.apache.org/licenses/LICENSE-2.0
8 : : //
9 : : // Unless required by applicable law or agreed to in writing, software
10 : : // distributed under the License is distributed on an "AS IS" BASIS,
11 : : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 : : // See the License for the specific language governing permissions and
13 : : // limitations under the License.
14 : :
15 : : #include "envpool/box2d/bipedal_walker_env.h"
16 : :
17 : : #include <algorithm>
18 : : #include <cmath>
19 : : #include <memory>
20 : : #include <stdexcept>
21 : : #include <utility>
22 : : #include <vector>
23 : :
24 : : #include "envpool/box2d/utils.h"
25 : : #include "opencv2/opencv.hpp"
26 : :
27 : : namespace box2d {
28 : :
29 : 3912033 : float BipedalWalkerLidarCallback::ReportFixture(b2Fixture* fixture,
30 : : const b2Vec2& point,
31 : : const b2Vec2& normal,
32 : : float frac) {
33 [ + + ]: 3912033 : if ((fixture->GetFilterData().categoryBits & 1) == 0) {
34 : : return -1;
35 : : }
36 : 1534196 : fraction = frac;
37 : 1534196 : return frac;
38 : : }
39 : :
40 : 296 : BipedalWalkerContactDetector::BipedalWalkerContactDetector(
41 : 296 : BipedalWalkerBox2dEnv* env)
42 : 296 : : env_(env) {}
43 : :
44 [ + - ]: 33915 : void BipedalWalkerContactDetector::BeginContact(b2Contact* contact) {
45 : : b2Body* body_a = contact->GetFixtureA()->GetBody();
46 : : b2Body* body_b = contact->GetFixtureB()->GetBody();
47 [ + - + + ]: 33915 : if (env_->hull_ == body_a || env_->hull_ == body_b) {
48 : 250 : env_->done_ = true;
49 : : }
50 [ + - + + ]: 33915 : if (env_->legs_[1] == body_a || env_->legs_[1] == body_b) {
51 : 16081 : env_->ground_contact_[1] = 1;
52 : : }
53 [ + + + + ]: 33915 : if (env_->legs_[3] == body_a || env_->legs_[3] == body_b) {
54 : 14915 : env_->ground_contact_[3] = 1;
55 : : }
56 : 33915 : }
57 : :
58 [ + - ]: 33458 : void BipedalWalkerContactDetector::EndContact(b2Contact* contact) {
59 : : b2Body* body_a = contact->GetFixtureA()->GetBody();
60 : : b2Body* body_b = contact->GetFixtureB()->GetBody();
61 [ + - + + ]: 33458 : if (env_->legs_[1] == body_a || env_->legs_[1] == body_b) {
62 : 16008 : env_->ground_contact_[1] = 0;
63 : : }
64 [ + + + + ]: 33458 : if (env_->legs_[3] == body_a || env_->legs_[3] == body_b) {
65 : 14820 : env_->ground_contact_[3] = 0;
66 : : }
67 : 33458 : }
68 : :
69 : 243 : BipedalWalkerBox2dEnv::BipedalWalkerBox2dEnv(bool hardcore,
70 : 243 : int max_episode_steps)
71 : 243 : : max_episode_steps_(max_episode_steps),
72 : 243 : elapsed_step_(max_episode_steps + 1),
73 : 243 : hardcore_(hardcore),
74 [ + - + - ]: 243 : world_(new b2World(b2Vec2(0.0, -10.0))) {
75 [ + + ]: 1451 : for (const auto& p : kHullPoly) {
76 [ + - + - : 1208 : hull_poly_.emplace_back(Vec2(p[0] / kScaleDouble, p[1] / kScaleDouble));
- - ]
77 : : }
78 : 243 : }
79 : :
80 : 63 : std::pair<int, int> BipedalWalkerBox2dEnv::RenderSize(int width,
81 : : int height) const {
82 [ + + ]: 63 : return {width > 0 ? width : static_cast<int>(kViewportW),
83 [ + + ]: 63 : height > 0 ? height : static_cast<int>(kViewportH)};
84 : : }
85 : :
86 : 849475 : std::array<float, 7> BipedalWalkerBox2dEnv::BodyState(
87 : : const b2Body* body) const {
88 : 849475 : const auto pos = body->GetPosition();
89 : 849475 : const auto vel = body->GetLinearVelocity();
90 : 849475 : return {pos.x,
91 : 849475 : pos.y,
92 : : body->GetAngle(),
93 : 849475 : vel.x,
94 [ + + ]: 849475 : vel.y,
95 : : body->GetAngularVelocity(),
96 [ + + ]: 849475 : body->IsAwake() ? 1.0f : 0.0f};
97 : : }
98 : :
99 [ + - ]: 170429 : std::vector<float> BipedalWalkerBox2dEnv::CloudPolyState() const {
100 : : std::vector<float> data;
101 [ + - ]: 170429 : data.reserve(cloud_poly_.size() * 5 * 2);
102 [ + + ]: 1881992 : for (const auto& cloud : cloud_poly_) {
103 [ + + ]: 10163559 : for (const auto& point : cloud.points) {
104 [ + - ]: 8452971 : data.emplace_back(point.x);
105 [ + - ]: 8453689 : data.emplace_back(point.y);
106 : : }
107 : : }
108 : 171598 : return data;
109 : 0 : }
110 : :
111 [ + - ]: 4080 : void BipedalWalkerBox2dEnv::CreateTerrain(std::vector<b2Vec2> poly) {
112 : : b2BodyDef bd;
113 : : bd.type = b2_staticBody;
114 : :
115 : : b2PolygonShape shape;
116 [ + - ]: 4080 : shape.Set(poly.data(), poly.size());
117 : :
118 : : b2FixtureDef fd;
119 : 4070 : fd.shape = &shape;
120 [ + - ]: 4070 : fd.friction = kFriction;
121 : :
122 [ + - ]: 4070 : auto* t = world_->CreateBody(&bd);
123 [ + - ]: 4071 : t->CreateFixture(&fd);
124 [ + - ]: 4076 : terrain_.emplace_back(t);
125 : 4075 : }
126 : :
127 : 296 : void BipedalWalkerBox2dEnv::ResetBox2d(std::mt19937* gen) {
128 : : // clean all body in world
129 [ + + ]: 296 : if (hull_ != nullptr) {
130 : 54 : world_->SetContactListener(nullptr);
131 [ + + ]: 11425 : for (auto& t : terrain_) {
132 : 11371 : world_->DestroyBody(t);
133 : : }
134 : : terrain_.clear();
135 : 54 : world_->DestroyBody(hull_);
136 [ + + ]: 270 : for (auto& l : legs_) {
137 : 216 : world_->DestroyBody(l);
138 : : }
139 : : }
140 : 296 : scroll_ = 0;
141 : 591 : listener_ = std::make_unique<BipedalWalkerContactDetector>(this);
142 : 295 : world_->SetContactListener(listener_.get());
143 : :
144 : : // terrain
145 : : {
146 : : int state = kGrass;
147 : : double velocity = 0.0;
148 : 292 : double y = kTerrainHeight;
149 : : int counter = kTerrainStartpad;
150 : : bool oneshot = false;
151 : : std::vector<double> terrain_x;
152 : : std::vector<double> terrain_y;
153 : : double original_y = 0.0;
154 : : int stair_steps = 0;
155 : : int stair_width = 0;
156 : : int stair_height = 0;
157 [ + + ]: 58956 : for (int i = 0; i < kTerrainLength; ++i) {
158 : 58660 : double x = i * kTerrainStep;
159 [ + - ]: 58660 : terrain_x.emplace_back(x);
160 [ + + ]: 58851 : if (state == kGrass && !oneshot) {
161 [ + - ]: 40205 : velocity = 0.8 * velocity + 0.01 * Sign(kTerrainHeight - y);
162 [ + + ]: 40209 : if (i > kTerrainStartpad) {
163 : 34256 : velocity += RandUniform(-1, 1)(*gen) / kScaleDouble;
164 : : }
165 : 40111 : y += velocity;
166 [ + + ]: 18646 : } else if (state == kPit && oneshot) {
167 : 624 : counter = RandInt(3, 4)(*gen);
168 [ + - + - ]: 624 : std::vector<b2Vec2> poly{Vec2(x, y), Vec2(x + kTerrainStep, y),
169 [ + - ]: 623 : Vec2(x + kTerrainStep, y - 4 * kTerrainStep),
170 [ + - + - ]: 624 : Vec2(x, y - 4 * kTerrainStep)};
171 [ + - + - ]: 625 : CreateTerrain(poly);
172 [ + + ]: 3120 : for (auto& p : poly) {
173 [ + - ]: 2496 : p = Vec2(p.x + kTerrainStep * counter, p.y);
174 : : }
175 [ + - + - ]: 624 : CreateTerrain(poly);
176 : 625 : counter += 2;
177 : : original_y = y;
178 [ + + ]: 18647 : } else if (state == kPit && !oneshot) {
179 : 2808 : y = original_y;
180 [ + + ]: 2808 : if (counter > 1) {
181 : 2190 : y -= 4 * kTerrainStep;
182 : : }
183 [ + + ]: 15214 : } else if (state == kStump && oneshot) {
184 : 662 : counter = RandInt(1, 2)(*gen);
185 : 662 : auto size = kTerrainStep * counter;
186 [ + - + - ]: 662 : std::vector<b2Vec2> poly{Vec2(x, y), Vec2(x + size, y),
187 [ + - + - : 662 : Vec2(x + size, y + size), Vec2(x, y + size)};
+ - ]
188 [ + - + - ]: 662 : CreateTerrain(poly);
189 [ + + ]: 15214 : } else if (state == kStairs && oneshot) {
190 [ + + ]: 624 : stair_height = RandUniform(0, 1)(*gen) > 0.5 ? 1 : -1;
191 : : stair_width = 4;
192 : 624 : stair_steps = RandInt(3, 4)(*gen);
193 : 624 : original_y = y;
194 [ + + ]: 2791 : for (int s = 0; s < stair_steps; ++s) {
195 : : std::vector<b2Vec2> poly{
196 : 2167 : Vec2(x + kTerrainStep * s * stair_width,
197 [ + - ]: 2167 : y + kTerrainStep * s * stair_height),
198 : 2168 : Vec2(x + kTerrainStep * (1 + s) * stair_width,
199 [ + - ]: 2168 : y + kTerrainStep * s * stair_height),
200 : 2172 : Vec2(x + kTerrainStep * (1 + s) * stair_width,
201 [ + - ]: 2172 : y + kTerrainStep * (s * stair_height - 1)),
202 : 2172 : Vec2(x + kTerrainStep * s * stair_width,
203 [ + - + - ]: 2172 : y + kTerrainStep * (s * stair_height - 1))};
204 [ + - + - ]: 2171 : CreateTerrain(poly);
205 : 2168 : }
206 : 624 : counter = stair_steps * stair_width;
207 [ + + ]: 13928 : } else if (state == kStairs && !oneshot) {
208 : 7721 : int s = stair_steps * stair_width - counter - stair_height;
209 : 7721 : y = original_y + kTerrainStep * s / stair_width * stair_height;
210 : : }
211 : : oneshot = false;
212 [ + - ]: 58758 : terrain_y.emplace_back(y);
213 [ + + ]: 58669 : if (--counter == 0) {
214 : 7820 : counter = RandInt(kTerrainGrass / 2, kTerrainGrass - 1)(*gen);
215 [ + + + + ]: 7820 : if (state == kGrass && hardcore_) {
216 : 1913 : state = RandInt(1, kStates - 1)(*gen);
217 : : } else {
218 : : state = kGrass;
219 : : }
220 : : oneshot = true;
221 : : }
222 : : }
223 [ + + ]: 58737 : for (std::size_t i = 0; i < terrain_x.size() - 1; ++i) {
224 : : b2BodyDef bd;
225 : : bd.type = b2_staticBody;
226 : :
227 : : b2EdgeShape shape;
228 [ + - + - ]: 58385 : shape.Set(Vec2(terrain_x[i], terrain_y[i]),
229 [ + - + - ]: 116876 : Vec2(terrain_x[i + 1], terrain_y[i + 1]));
230 : :
231 : : b2FixtureDef fd;
232 : 58435 : fd.shape = &shape;
233 : 58435 : fd.friction = kFriction;
234 [ + - ]: 58435 : fd.filter.categoryBits = 0x0001;
235 : :
236 [ + - ]: 58435 : auto* t = world_->CreateBody(&bd);
237 [ + - ]: 58202 : t->CreateFixture(&fd);
238 [ + - ]: 58513 : terrain_.emplace_back(t);
239 : : }
240 : : std::reverse(terrain_.begin(), terrain_.end());
241 : 296 : }
242 : :
243 : : cloud_poly_.clear();
244 [ + + ]: 3254 : for (int i = 0; i < kTerrainLength / 20; ++i) {
245 : 2956 : float x = RandUniform(0, kTerrainLength)(*gen) * kTerrainStep;
246 : 2956 : auto y = static_cast<float>(kViewportH / kScaleDouble * 3 / 4);
247 : : BipedalWalkerCloudPoly cloud;
248 : 2956 : cloud.x1 = x;
249 : 2956 : cloud.x2 = x;
250 [ + + ]: 17663 : for (int a = 0; a < 5; ++a) {
251 : 14708 : float px = x + 15 * kTerrainStep * std::sin(3.14f * 2 * a / 5) +
252 : 14708 : RandUniform(0, 5 * kTerrainStep)(*gen);
253 : 14715 : float py = y + 5 * kTerrainStep * std::cos(3.14f * 2 * a / 5) +
254 : 14710 : RandUniform(0, 5 * kTerrainStep)(*gen);
255 [ + + ]: 14710 : cloud.points[a] = Vec2(px, py);
256 [ + + ]: 14707 : cloud.x1 = std::min(cloud.x1, px);
257 : 14707 : cloud.x2 = std::max(cloud.x2, px);
258 : : }
259 : 2955 : cloud_poly_.emplace_back(cloud);
260 : : }
261 : :
262 : : // hull
263 : 296 : double init_x = kTerrainStep * kTerrainStartpad / 2;
264 : 296 : double init_y = kTerrainHeight + 2 * kLegH;
265 : : {
266 : : b2BodyDef bd;
267 : 296 : bd.type = b2_dynamicBody;
268 : 296 : bd.position = Vec2(init_x, init_y);
269 : :
270 : : b2PolygonShape shape;
271 : 296 : shape.Set(hull_poly_.data(), hull_poly_.size());
272 : :
273 : : b2FixtureDef fd;
274 : 296 : fd.shape = &shape;
275 : 296 : fd.density = 5.0;
276 : 296 : fd.friction = 0.1;
277 : 296 : fd.filter.categoryBits = 0x0020;
278 : 296 : fd.filter.maskBits = 0x001;
279 : : fd.restitution = 0.0;
280 : :
281 : 296 : hull_ = world_->CreateBody(&bd);
282 : 296 : hull_->CreateFixture(&fd);
283 : 296 : b2Vec2 force = Vec2(RandUniform(-kInitialRandom, kInitialRandom)(*gen), 0);
284 : 296 : initial_force_ = force.x;
285 [ + - ]: 296 : hull_->ApplyForceToCenter(force, true);
286 : : }
287 : :
288 : : // leg
289 [ + + ]: 887 : for (int index = 0; index < 2; ++index) {
290 [ + + ]: 591 : float sign = index == 0 ? -1 : 1;
291 : :
292 : : // upper leg
293 : : b2BodyDef bd;
294 : 591 : bd.type = b2_dynamicBody;
295 : 591 : bd.position = Vec2(init_x, init_y - kLegH / 2 - kLegDown);
296 : 591 : bd.angle = sign * 0.05f;
297 : :
298 : : b2PolygonShape shape;
299 : 591 : shape.SetAsBox(static_cast<float>(kLegW / 2),
300 : 591 : static_cast<float>(kLegH / 2));
301 : :
302 : : b2FixtureDef fd;
303 : 588 : fd.shape = &shape;
304 : 588 : fd.density = 1.0;
305 : 588 : fd.filter.categoryBits = 0x0020;
306 : 588 : fd.filter.maskBits = 0x001;
307 : : fd.restitution = 0.0;
308 : :
309 : 588 : legs_[index * 2] = world_->CreateBody(&bd);
310 : 590 : legs_[index * 2]->CreateFixture(&fd);
311 : 591 : ground_contact_[index * 2] = 0;
312 : :
313 : : b2RevoluteJointDef rjd;
314 : 591 : rjd.bodyA = hull_;
315 : 591 : rjd.bodyB = legs_[index * 2];
316 : 591 : rjd.localAnchorA = Vec2(0, kLegDown);
317 : 591 : rjd.localAnchorB = Vec2(0, kLegH / 2);
318 : 591 : rjd.referenceAngle = rjd.bodyB->GetAngle() - rjd.bodyA->GetAngle();
319 : 591 : rjd.enableMotor = true;
320 : 591 : rjd.enableLimit = true;
321 : 591 : rjd.maxMotorTorque = kMotorsTorque;
322 : 591 : rjd.motorSpeed = sign;
323 : 591 : rjd.lowerAngle = -0.8;
324 : 591 : rjd.upperAngle = 1.1;
325 : 591 : rjd.type = b2JointType::e_revoluteJoint;
326 : 590 : joints_[index * 2] =
327 : 591 : static_cast<b2RevoluteJoint*>(world_->CreateJoint(&rjd));
328 : :
329 : : // lower leg
330 : 590 : bd.position = Vec2(init_x, init_y - kLegH * 3 / 2 - kLegDown);
331 : 588 : shape.SetAsBox(static_cast<float>(0.8 * kLegW / 2),
332 : 588 : static_cast<float>(kLegH / 2));
333 : 589 : legs_[index * 2 + 1] = world_->CreateBody(&bd);
334 : 589 : legs_[index * 2 + 1]->CreateFixture(&fd);
335 : 590 : ground_contact_[index * 2 + 1] = 0;
336 : :
337 : 590 : rjd.bodyA = legs_[index * 2];
338 : 590 : rjd.bodyB = legs_[index * 2 + 1];
339 : 590 : rjd.localAnchorA = Vec2(0, -kLegH / 2);
340 : 592 : rjd.localAnchorB = Vec2(0, kLegH / 2);
341 : 590 : rjd.referenceAngle = rjd.bodyB->GetAngle() - rjd.bodyA->GetAngle();
342 : 590 : rjd.motorSpeed = 1;
343 : 590 : rjd.lowerAngle = -1.6;
344 : 590 : rjd.upperAngle = -0.1;
345 : 591 : joints_[index * 2 + 1] =
346 : 590 : static_cast<b2RevoluteJoint*>(world_->CreateJoint(&rjd));
347 : : }
348 : 296 : }
349 : :
350 : 171473 : void BipedalWalkerBox2dEnv::StepBox2d(std::mt19937* gen, float action0,
351 : : float action1, float action2,
352 : : float action3) {
353 [ - + ]: 171473 : float clip0 = std::min(std::max(std::abs(action0), 0.0f), 1.0f);
354 [ - + ]: 171473 : float clip1 = std::min(std::max(std::abs(action1), 0.0f), 1.0f);
355 [ - + ]: 171473 : float clip2 = std::min(std::max(std::abs(action2), 0.0f), 1.0f);
356 [ - + ]: 171473 : float clip3 = std::min(std::max(std::abs(action3), 0.0f), 1.0f);
357 : 171473 : joints_[0]->SetMotorSpeed(kSpeedHip * Sign(action0));
358 : 171560 : joints_[1]->SetMotorSpeed(kSpeedKnee * Sign(action1));
359 : 171472 : joints_[2]->SetMotorSpeed(kSpeedHip * Sign(action2));
360 : 171382 : joints_[3]->SetMotorSpeed(kSpeedKnee * Sign(action3));
361 : 171350 : joints_[0]->SetMaxMotorTorque(kMotorsTorque * clip0);
362 : 171250 : joints_[1]->SetMaxMotorTorque(kMotorsTorque * clip1);
363 : 171287 : joints_[2]->SetMaxMotorTorque(kMotorsTorque * clip2);
364 : 171308 : joints_[3]->SetMaxMotorTorque(kMotorsTorque * clip3);
365 : :
366 : 171359 : world_->Step(static_cast<float>(1.0 / kFPS), 6 * 30, 2 * 30);
367 : :
368 : 171796 : auto pos = hull_->GetPosition();
369 : 171796 : auto vel = hull_->GetLinearVelocity();
370 : :
371 [ + + ]: 1887339 : for (int i = 0; i < kLidarNum; ++i) {
372 : 1715518 : lidar_[i].fraction = 1.0;
373 : 1713744 : world_->RayCast(&lidar_[i], pos,
374 : 1715543 : Vec2(pos.x + std::sin(1.5 * i / 10.0) * kLidarRange,
375 : 1715518 : pos.y - std::cos(1.5 * i / 10.0) * kLidarRange));
376 : : }
377 : :
378 : 171821 : obs_[0] = hull_->GetAngle();
379 : 171821 : obs_[1] = 2.0 * hull_->GetAngularVelocity() / kFPS;
380 : 171821 : obs_[2] = 0.3 * vel.x * kViewportW / kScaleDouble / kFPS;
381 : 171821 : obs_[3] = 0.3 * vel.y * kViewportH / kScaleDouble / kFPS;
382 : 171821 : obs_[4] = joints_[0]->GetJointAngle();
383 : 171684 : obs_[5] = joints_[0]->GetJointSpeed() / kSpeedHip;
384 : 171688 : obs_[6] = joints_[1]->GetJointAngle() + 1.0;
385 : 171743 : obs_[7] = joints_[1]->GetJointSpeed() / kSpeedKnee;
386 : 171748 : obs_[8] = ground_contact_[1];
387 : 171748 : obs_[9] = joints_[2]->GetJointAngle();
388 : 171741 : obs_[10] = joints_[2]->GetJointSpeed() / kSpeedHip;
389 : 171716 : obs_[11] = joints_[3]->GetJointAngle() + 1.0;
390 : 171745 : obs_[12] = joints_[3]->GetJointSpeed() / kSpeedKnee;
391 : 171671 : obs_[13] = ground_contact_[3];
392 [ + + ]: 1888561 : for (int i = 0; i < kLidarNum; ++i) {
393 : 1716890 : obs_[14 + i] = lidar_[i].fraction;
394 : : }
395 : :
396 [ + + ]: 171671 : auto shaping = 130 * pos.x / kScaleFloat - 5 * std::abs(obs_[0]);
397 : 171671 : reward_ = 0;
398 [ + + ]: 171671 : if (elapsed_step_ > 0) {
399 : 171382 : reward_ = shaping - prev_shaping_;
400 : : }
401 : 171671 : prev_shaping_ = shaping;
402 : 171671 : reward_ -= 0.00035f * kMotorsTorque * (clip0 + clip1 + clip2 + clip3);
403 : :
404 [ + + - + ]: 171671 : if (done_ || pos.x < 0) {
405 : 231 : done_ = true;
406 : 231 : reward_ = -100;
407 : : }
408 [ + + ]: 171671 : if (pos.x > (kTerrainLength - kTerrainGrass) * kTerrainStep) {
409 : 11 : done_ = true;
410 : : }
411 [ + + ]: 171671 : if (elapsed_step_ >= max_episode_steps_) {
412 : 14 : done_ = true;
413 : : }
414 : :
415 [ + + ]: 171671 : scroll_ = pos.x - static_cast<float>(kViewportW / kScaleDouble / 5);
416 : : // info
417 : : terrain_edge_path2_.clear();
418 : : terrain_poly_path4_.clear();
419 : : leg_path4_.clear();
420 : : hull_path5_.clear();
421 : : // terrain
422 [ + + + + ]: 34718871 : for (auto* b : terrain_) {
423 : : b2Fixture& f = b->GetFixtureList()[0];
424 [ + + ]: 34547065 : auto trans = f.GetBody()->GetTransform();
425 [ + + ]: 34547065 : if (f.GetShape()->GetType() == 1) {
426 : : auto* shape = static_cast<b2EdgeShape*>(f.GetShape());
427 : 32987007 : b2Vec2 v1 = kScaleFloat * b2Mul(trans, shape->m_vertex1);
428 : 32987007 : b2Vec2 v2 = kScaleFloat * b2Mul(trans, shape->m_vertex2);
429 : 32987007 : terrain_edge_path2_.emplace_back(v1.x);
430 : 32870105 : terrain_edge_path2_.emplace_back(v1.y);
431 : 32924370 : terrain_edge_path2_.emplace_back(v2.x);
432 : 32980879 : terrain_edge_path2_.emplace_back(v2.y);
433 : : } else {
434 : : auto* shape = static_cast<b2PolygonShape*>(f.GetShape());
435 [ + + ]: 8471427 : for (int i = 0; i < shape->m_count; ++i) {
436 : 6852943 : b2Vec2 v = kScaleFloat * b2Mul(trans, shape->m_vertices[i]);
437 : 6852943 : terrain_poly_path4_.emplace_back(v.x);
438 : 6845004 : terrain_poly_path4_.emplace_back(v.y);
439 : : }
440 : : }
441 : : }
442 : : // legs
443 [ + + ]: 858397 : for (auto* b : legs_) {
444 : : b2Fixture& f = b->GetFixtureList()[0];
445 : 686572 : auto trans = f.GetBody()->GetTransform();
446 : : auto* shape = static_cast<b2PolygonShape*>(f.GetShape());
447 [ + + ]: 3426146 : for (int i = 0; i < shape->m_count; ++i) {
448 [ + - ]: 2739555 : b2Vec2 v = kScaleFloat * b2Mul(trans, shape->m_vertices[i]);
449 [ + - ]: 2739555 : leg_path4_.emplace_back(v.x);
450 [ + - ]: 2738637 : leg_path4_.emplace_back(v.y);
451 : : }
452 : : }
453 : : // hull
454 : 171825 : b2Fixture& f = hull_->GetFixtureList()[0];
455 : 171825 : auto trans = f.GetBody()->GetTransform();
456 : : auto* shape = static_cast<b2PolygonShape*>(f.GetShape());
457 [ + + ]: 1029907 : for (int i = 0; i < shape->m_count; ++i) {
458 : 858183 : b2Vec2 v = kScaleFloat * b2Mul(trans, shape->m_vertices[i]);
459 : 858183 : hull_path5_.emplace_back(v.x);
460 : 858011 : hull_path5_.emplace_back(v.y);
461 : : }
462 : 171724 : }
463 : :
464 : 293 : void BipedalWalkerBox2dEnv::BipedalWalkerReset(std::mt19937* gen) {
465 : 293 : elapsed_step_ = 0;
466 : 293 : done_ = false;
467 : 293 : ResetBox2d(gen);
468 : 295 : StepBox2d(gen, 0, 0, 0, 0);
469 : 296 : }
470 : :
471 : 171286 : void BipedalWalkerBox2dEnv::BipedalWalkerStep(std::mt19937* gen, float action0,
472 : : float action1, float action2,
473 : : float action3) {
474 : 171286 : ++elapsed_step_;
475 : 171286 : StepBox2d(gen, action0, action1, action2, action3);
476 : 171455 : }
477 : :
478 : 33 : void BipedalWalkerBox2dEnv::Render(int width, int height, int /*camera_id*/,
479 : : unsigned char* rgb) {
480 [ - + ]: 33 : if (hull_ == nullptr) {
481 [ # # ]: 0 : throw std::runtime_error("render called before BipedalWalker reset");
482 : : }
483 : :
484 : : auto to_point = [](float x, float y) {
485 [ + - + - : 8562 : return cv::Point(static_cast<int>(std::lround(x)),
+ - + - +
- + - ]
486 : : static_cast<int>(std::lround(y)));
487 : : };
488 : :
489 [ - + ]: 33 : const auto scroll_px = static_cast<int>(std::lround(scroll_ * kScaleFloat));
490 : 33 : const auto viewport_w = static_cast<int>(kViewportW);
491 : 33 : const auto viewport_h = static_cast<int>(kViewportH);
492 [ - + + - ]: 66 : int surf_width = std::max(viewport_w, viewport_w + std::max(scroll_px, 0));
493 : :
494 : 33 : cv::Mat surf(viewport_h, surf_width, CV_8UC3, cv::Scalar(255, 215, 215));
495 : :
496 : : std::vector<cv::Point> background = {
497 [ + - ]: 33 : to_point(static_cast<float>(scroll_px), 0.0f),
498 : 33 : to_point(static_cast<float>(scroll_px + viewport_w), 0.0f),
499 : 33 : to_point(static_cast<float>(scroll_px + viewport_w),
500 : : static_cast<float>(viewport_h)),
501 : 33 : to_point(static_cast<float>(scroll_px), static_cast<float>(viewport_h)),
502 [ + - ]: 33 : };
503 [ + - + - ]: 33 : cv::fillConvexPoly(surf, background, cv::Scalar(255, 215, 215));
504 : :
505 [ + + ]: 363 : for (const auto& cloud : cloud_poly_) {
506 [ - + ]: 330 : if (cloud.x2 < scroll_ / 2) {
507 : 285 : continue;
508 : : }
509 : 615 : if (cloud.x1 >
510 [ + + ]: 330 : scroll_ / 2 + static_cast<float>(kViewportW / kScaleDouble)) {
511 : 285 : continue;
512 : : }
513 : : std::vector<cv::Point> polygon;
514 [ + - ]: 45 : polygon.reserve(cloud.points.size());
515 [ + + ]: 270 : for (const auto& point : cloud.points) {
516 : 225 : polygon.push_back(to_point(point.x * kScaleFloat + scroll_px / 2.0f,
517 [ + - ]: 225 : point.y * kScaleFloat));
518 : : }
519 [ + - + - ]: 45 : cv::fillConvexPoly(surf, polygon, cv::Scalar(255, 255, 255));
520 [ + - + - ]: 45 : cv::polylines(surf, polygon, true, cv::Scalar(255, 255, 255), 1,
521 : : cv::LINE_AA);
522 : 45 : }
523 : :
524 [ + + ]: 6600 : for (std::size_t i = 0; i + 3 < terrain_edge_path2_.size(); i += 4) {
525 : : std::vector<cv::Point> polygon = {
526 [ + - ]: 6567 : to_point(terrain_edge_path2_[i], terrain_edge_path2_[i + 1]),
527 : 6567 : to_point(terrain_edge_path2_[i + 2], terrain_edge_path2_[i + 3]),
528 : 6567 : to_point(terrain_edge_path2_[i + 2], 0.0f),
529 : 6567 : to_point(terrain_edge_path2_[i], 0.0f),
530 [ + - ]: 6567 : };
531 [ + - + - ]: 6567 : cv::fillConvexPoly(surf, polygon, cv::Scalar(76, 153, 102));
532 [ + - ]: 6567 : cv::line(surf, to_point(terrain_edge_path2_[i], terrain_edge_path2_[i + 1]),
533 : : to_point(terrain_edge_path2_[i + 2], terrain_edge_path2_[i + 3]),
534 : 6567 : cv::Scalar(0, 255, 0), 1, cv::LINE_AA);
535 : 6567 : }
536 : :
537 [ + + ]: 294 : for (std::size_t i = 0; i + 7 < terrain_poly_path4_.size(); i += 8) {
538 : : std::vector<cv::Point> polygon;
539 [ + - ]: 261 : polygon.reserve(4);
540 [ + + ]: 1305 : for (std::size_t j = 0; j < 8; j += 2) {
541 : : polygon.push_back(
542 [ + - ]: 1044 : to_point(terrain_poly_path4_[i + j], terrain_poly_path4_[i + j + 1]));
543 : : }
544 [ + - + - ]: 261 : cv::fillConvexPoly(surf, polygon, cv::Scalar(255, 255, 255));
545 [ + - + - ]: 261 : cv::polylines(surf, polygon, true, cv::Scalar(153, 153, 153), 1,
546 : : cv::LINE_AA);
547 : 261 : }
548 : :
549 : : auto leg_fill_color = [](int sign) {
550 : 132 : return cv::Scalar(127 - sign * 25, 76 - sign * 25, 153 - sign * 25);
551 : : };
552 : : auto leg_outline_color = [](int sign) {
553 : 132 : return cv::Scalar(76 - sign * 25, 51 - sign * 25, 102 - sign * 25);
554 : : };
555 [ + + ]: 165 : for (std::size_t i = 0; i + 7 < leg_path4_.size(); i += 8) {
556 : : std::vector<cv::Point> polygon;
557 [ + - ]: 132 : polygon.reserve(4);
558 [ + + ]: 660 : for (std::size_t j = 0; j < 8; j += 2) {
559 [ + - ]: 528 : polygon.push_back(to_point(leg_path4_[i + j], leg_path4_[i + j + 1]));
560 : : }
561 : 132 : const auto leg_index = static_cast<int>(i / 8);
562 [ + + ]: 132 : int sign = leg_index < 2 ? -1 : 1;
563 [ + - + - ]: 132 : cv::fillConvexPoly(surf, polygon, leg_fill_color(sign));
564 [ + - + - ]: 132 : cv::polylines(surf, polygon, true, leg_outline_color(sign), 1, cv::LINE_AA);
565 : 132 : }
566 : :
567 [ + - ]: 33 : if (!hull_path5_.empty()) {
568 : : std::vector<cv::Point> hull;
569 [ + - ]: 33 : hull.reserve(hull_path5_.size() / 2);
570 [ + + ]: 198 : for (std::size_t i = 0; i + 1 < hull_path5_.size(); i += 2) {
571 [ + - ]: 165 : hull.push_back(to_point(hull_path5_[i], hull_path5_[i + 1]));
572 : : }
573 [ + - + - ]: 33 : cv::fillConvexPoly(surf, hull, cv::Scalar(229, 51, 127));
574 [ + - + - ]: 33 : cv::polylines(surf, hull, true, cv::Scalar(127, 76, 76), 1, cv::LINE_AA);
575 : 33 : }
576 : :
577 : 33 : auto flag_x = static_cast<float>(kTerrainStep * 3 * kScaleFloat);
578 : 33 : auto flag_y1 = static_cast<float>(kTerrainHeight * kScaleFloat);
579 : 33 : float flag_y2 = flag_y1 + 50.0f;
580 [ + - ]: 33 : cv::line(surf, to_point(flag_x, flag_y1), to_point(flag_x, flag_y2),
581 [ + - ]: 33 : cv::Scalar(0, 0, 0), 1, cv::LINE_AA);
582 : : std::vector<cv::Point> flag = {
583 : 33 : to_point(flag_x, flag_y2),
584 : 33 : to_point(flag_x, flag_y2 - 10.0f),
585 : 33 : to_point(flag_x + 25.0f, flag_y2 - 5.0f),
586 [ + - ]: 33 : };
587 [ + - + - ]: 33 : cv::fillConvexPoly(surf, flag, cv::Scalar(0, 51, 230));
588 [ + - + - : 33 : cv::polylines(surf, flag, true, cv::Scalar(0, 0, 0), 1, cv::LINE_AA);
+ - ]
589 : :
590 [ + - ]: 33 : cv::flip(surf, surf, 0);
591 : :
592 [ + - ]: 33 : cv::Mat view(viewport_h, viewport_w, CV_8UC3, cv::Scalar(255, 215, 215));
593 [ - + ]: 33 : int src_x = std::max(scroll_px, 0);
594 [ + - ]: 33 : int dst_x = std::max(-scroll_px, 0);
595 [ - + ]: 33 : int copy_width = std::min(surf.cols - src_x, viewport_w - dst_x);
596 [ + - ]: 33 : if (copy_width > 0) {
597 : 33 : surf(cv::Rect(src_x, 0, copy_width, viewport_h))
598 [ + - ]: 33 : .copyTo(view(cv::Rect(dst_x, 0, copy_width, viewport_h)));
599 : : }
600 : :
601 [ + - ]: 33 : cv::Mat output(height, width, CV_8UC3, rgb);
602 [ + + + - ]: 33 : if (width == viewport_w && height == viewport_h) {
603 [ + - ]: 18 : cv::cvtColor(view, output, cv::COLOR_BGR2RGB);
604 : : return;
605 : : }
606 : 15 : cv::Mat resized;
607 [ + - + - ]: 15 : cv::resize(view, resized, cv::Size(width, height));
608 [ + - ]: 15 : cv::cvtColor(resized, output, cv::COLOR_BGR2RGB);
609 : 33 : }
610 : :
611 : : } // namespace box2d
|
