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 : 6078741 : float BipedalWalkerLidarCallback::ReportFixture(b2Fixture* fixture,
30 : : const b2Vec2& point,
31 : : const b2Vec2& normal,
32 : : float frac) {
33 [ + + ]: 6078741 : if ((fixture->GetFilterData().categoryBits & 1) == 0) {
34 : : return -1;
35 : : }
36 : 2412820 : fraction = frac;
37 : 2412820 : return frac;
38 : : }
39 : :
40 : 432 : BipedalWalkerContactDetector::BipedalWalkerContactDetector(
41 : 432 : BipedalWalkerBox2dEnv* env)
42 : 432 : : env_(env) {}
43 : :
44 [ + + ]: 73212 : void BipedalWalkerContactDetector::BeginContact(b2Contact* contact) {
45 : : b2Body* body_a = contact->GetFixtureA()->GetBody();
46 : : b2Body* body_b = contact->GetFixtureB()->GetBody();
47 [ + + + + ]: 73212 : if (env_->hull_ == body_a || env_->hull_ == body_b) {
48 : 341 : env_->done_ = true;
49 : : }
50 [ + - + + ]: 73212 : if (env_->legs_[1] == body_a || env_->legs_[1] == body_b) {
51 : 34930 : env_->ground_contact_[1] = 1;
52 : : }
53 [ + + + + ]: 73212 : if (env_->legs_[3] == body_a || env_->legs_[3] == body_b) {
54 : 33252 : env_->ground_contact_[3] = 1;
55 : : }
56 : 73212 : }
57 : :
58 [ + - ]: 72480 : void BipedalWalkerContactDetector::EndContact(b2Contact* contact) {
59 : : b2Body* body_a = contact->GetFixtureA()->GetBody();
60 : : b2Body* body_b = contact->GetFixtureB()->GetBody();
61 [ + - + + ]: 72480 : if (env_->legs_[1] == body_a || env_->legs_[1] == body_b) {
62 : 34774 : env_->ground_contact_[1] = 0;
63 : : }
64 [ + + + + ]: 72480 : if (env_->legs_[3] == body_a || env_->legs_[3] == body_b) {
65 : 33076 : env_->ground_contact_[3] = 0;
66 : : }
67 : 72480 : }
68 : :
69 : 235 : BipedalWalkerBox2dEnv::BipedalWalkerBox2dEnv(bool hardcore,
70 : 235 : int max_episode_steps)
71 : 235 : : max_episode_steps_(max_episode_steps),
72 : 235 : elapsed_step_(max_episode_steps + 1),
73 : 235 : hardcore_(hardcore),
74 [ + - + - ]: 235 : world_(new b2World(b2Vec2(0.0, -10.0))) {
75 [ + + ]: 1417 : for (const auto& p : kHullPoly) {
76 [ + - + - : 1180 : hull_poly_.emplace_back(Vec2(p[0] / kScaleDouble, p[1] / kScaleDouble));
- - ]
77 : : }
78 : 237 : }
79 : :
80 : 55 : std::pair<int, int> BipedalWalkerBox2dEnv::RenderSize(int width,
81 : : int height) const {
82 [ + + ]: 55 : return {width > 0 ? width : static_cast<int>(kViewportW),
83 [ + + ]: 55 : height > 0 ? height : static_cast<int>(kViewportH)};
84 : : }
85 : :
86 : 6828 : void BipedalWalkerBox2dEnv::CreateTerrain(std::vector<b2Vec2> poly) {
87 : : b2BodyDef bd;
88 : : bd.type = b2_staticBody;
89 : :
90 : 6828 : b2PolygonShape shape;
91 : 6828 : shape.Set(poly.data(), poly.size());
92 : :
93 : : b2FixtureDef fd;
94 : 6830 : fd.shape = &shape;
95 : 6830 : fd.friction = kFriction;
96 : :
97 : 6830 : auto* t = world_->CreateBody(&bd);
98 : 6830 : t->CreateFixture(&fd);
99 : 6830 : terrain_.emplace_back(t);
100 : 6831 : }
101 : :
102 : 432 : void BipedalWalkerBox2dEnv::ResetBox2d(std::mt19937* gen) {
103 : : // clean all body in world
104 [ + + ]: 432 : if (hull_ != nullptr) {
105 : 196 : world_->SetContactListener(nullptr);
106 [ + + ]: 42653 : for (auto& t : terrain_) {
107 : 42457 : world_->DestroyBody(t);
108 : : }
109 : : terrain_.clear();
110 : 196 : world_->DestroyBody(hull_);
111 [ + + ]: 980 : for (auto& l : legs_) {
112 : 784 : world_->DestroyBody(l);
113 : : }
114 : : }
115 : 432 : scroll_ = 0;
116 : 864 : listener_ = std::make_unique<BipedalWalkerContactDetector>(this);
117 : 432 : world_->SetContactListener(listener_.get());
118 : :
119 : : // terrain
120 : : {
121 : : int state = kGrass;
122 : : double velocity = 0.0;
123 : 431 : double y = kTerrainHeight;
124 : : int counter = kTerrainStartpad;
125 : : bool oneshot = false;
126 : : std::vector<double> terrain_x;
127 : : std::vector<double> terrain_y;
128 : : double original_y = 0.0;
129 : : int stair_steps = 0;
130 : : int stair_width = 0;
131 : : int stair_height = 0;
132 [ + + ]: 86700 : for (int i = 0; i < kTerrainLength; ++i) {
133 : 86268 : double x = i * kTerrainStep;
134 [ + - ]: 86268 : terrain_x.emplace_back(x);
135 [ + + ]: 86330 : if (state == kGrass && !oneshot) {
136 [ + - ]: 56890 : velocity = 0.8 * velocity + 0.01 * Sign(kTerrainHeight - y);
137 [ + + ]: 56890 : if (i > kTerrainStartpad) {
138 : 48234 : velocity += RandUniform(-1, 1)(*gen) / kScaleDouble;
139 : : }
140 : 56867 : y += velocity;
141 [ + + ]: 29440 : } else if (state == kPit && oneshot) {
142 : 1040 : counter = RandInt(3, 4)(*gen);
143 [ + - + - ]: 1040 : std::vector<b2Vec2> poly{Vec2(x, y), Vec2(x + kTerrainStep, y),
144 [ + - ]: 1040 : Vec2(x + kTerrainStep, y - 4 * kTerrainStep),
145 [ + - + - ]: 1040 : Vec2(x, y - 4 * kTerrainStep)};
146 [ + - + - ]: 1040 : CreateTerrain(poly);
147 [ + + ]: 5200 : for (auto& p : poly) {
148 [ + - ]: 4160 : p = Vec2(p.x + kTerrainStep * counter, p.y);
149 : : }
150 [ + - + - ]: 1040 : CreateTerrain(poly);
151 : 1040 : counter += 2;
152 : : original_y = y;
153 [ + + ]: 29440 : } else if (state == kPit && !oneshot) {
154 : 4663 : y = original_y;
155 [ + + ]: 4663 : if (counter > 1) {
156 : 3644 : y -= 4 * kTerrainStep;
157 : : }
158 [ + + ]: 23737 : } else if (state == kStump && oneshot) {
159 : 1177 : counter = RandInt(1, 2)(*gen);
160 : 1177 : auto size = kTerrainStep * counter;
161 [ + - + - ]: 1177 : std::vector<b2Vec2> poly{Vec2(x, y), Vec2(x + size, y),
162 [ + - + - : 1177 : Vec2(x + size, y + size), Vec2(x, y + size)};
+ - ]
163 [ + - + - ]: 1177 : CreateTerrain(poly);
164 [ + + ]: 23737 : } else if (state == kStairs && oneshot) {
165 [ + + ]: 1022 : stair_height = RandUniform(0, 1)(*gen) > 0.5 ? 1 : -1;
166 : : stair_width = 4;
167 : 1022 : stair_steps = RandInt(3, 4)(*gen);
168 : 1022 : original_y = y;
169 [ + + ]: 4595 : for (int s = 0; s < stair_steps; ++s) {
170 : : std::vector<b2Vec2> poly{
171 : 3573 : Vec2(x + kTerrainStep * s * stair_width,
172 [ + - ]: 3573 : y + kTerrainStep * s * stair_height),
173 : 3574 : Vec2(x + kTerrainStep * (1 + s) * stair_width,
174 [ + - ]: 3574 : y + kTerrainStep * s * stair_height),
175 : 3573 : Vec2(x + kTerrainStep * (1 + s) * stair_width,
176 [ + - ]: 3573 : y + kTerrainStep * (s * stair_height - 1)),
177 : 3574 : Vec2(x + kTerrainStep * s * stair_width,
178 [ + - + - ]: 3574 : y + kTerrainStep * (s * stair_height - 1))};
179 [ + - + - ]: 3573 : CreateTerrain(poly);
180 : 3575 : }
181 : 1022 : counter = stair_steps * stair_width;
182 [ + + ]: 21538 : } else if (state == kStairs && !oneshot) {
183 : 12725 : int s = stair_steps * stair_width - counter - stair_height;
184 : 12725 : y = original_y + kTerrainStep * s / stair_width * stair_height;
185 : : }
186 : : oneshot = false;
187 [ + - ]: 86307 : terrain_y.emplace_back(y);
188 [ + + ]: 86268 : if (--counter == 0) {
189 : 11516 : counter = RandInt(kTerrainGrass / 2, kTerrainGrass - 1)(*gen);
190 [ + + + + ]: 11516 : if (state == kGrass && hardcore_) {
191 : 3255 : state = RandInt(1, kStates - 1)(*gen);
192 : : } else {
193 : : state = kGrass;
194 : : }
195 : : oneshot = true;
196 : : }
197 : : }
198 [ + + ]: 86343 : for (std::size_t i = 0; i < terrain_x.size() - 1; ++i) {
199 : : b2BodyDef bd;
200 : : bd.type = b2_staticBody;
201 : :
202 : : b2EdgeShape shape;
203 [ + - + - ]: 85896 : shape.SetTwoSided(Vec2(terrain_x[i], terrain_y[i]),
204 [ + - + - ]: 171809 : Vec2(terrain_x[i + 1], terrain_y[i + 1]));
205 : :
206 : : b2FixtureDef fd;
207 : 85898 : fd.shape = &shape;
208 : 85898 : fd.friction = kFriction;
209 [ + - ]: 85898 : fd.filter.categoryBits = 0x0001;
210 : :
211 [ + - ]: 85898 : auto* t = world_->CreateBody(&bd);
212 [ + - ]: 85904 : t->CreateFixture(&fd);
213 [ + - ]: 85905 : terrain_.emplace_back(t);
214 : : }
215 : : std::reverse(terrain_.begin(), terrain_.end());
216 : 432 : }
217 : :
218 : : cloud_poly_.clear();
219 [ + + ]: 4752 : for (int i = 0; i < kTerrainLength / 20; ++i) {
220 : 4318 : float x = RandUniform(0, kTerrainLength)(*gen) * kTerrainStep;
221 : 4318 : auto y = static_cast<float>(kViewportH / kScaleDouble * 3 / 4);
222 : : BipedalWalkerCloudPoly cloud;
223 : 4318 : cloud.x1 = x;
224 : 4318 : cloud.x2 = x;
225 [ + + ]: 25902 : for (int a = 0; a < 5; ++a) {
226 : 21582 : float px = x + 15 * kTerrainStep * std::sin(3.14f * 2 * a / 5) +
227 : 21582 : RandUniform(0, 5 * kTerrainStep)(*gen);
228 : 21580 : float py = y + 5 * kTerrainStep * std::cos(3.14f * 2 * a / 5) +
229 : 21585 : RandUniform(0, 5 * kTerrainStep)(*gen);
230 [ + + ]: 21585 : cloud.points[a] = Vec2(px, py);
231 [ + + ]: 21584 : cloud.x1 = std::min(cloud.x1, px);
232 : 21584 : cloud.x2 = std::max(cloud.x2, px);
233 : : }
234 : 4320 : cloud_poly_.emplace_back(cloud);
235 : : }
236 : :
237 : : // hull
238 : 432 : double init_x = kTerrainStep * kTerrainStartpad / 2;
239 : 432 : double init_y = kTerrainHeight + 2 * kLegH;
240 : : {
241 : : b2BodyDef bd;
242 : 432 : bd.type = b2_dynamicBody;
243 : 432 : bd.position = Vec2(init_x, init_y);
244 : :
245 : 432 : b2PolygonShape shape;
246 : 432 : shape.Set(hull_poly_.data(), hull_poly_.size());
247 : :
248 : : b2FixtureDef fd;
249 : 432 : fd.shape = &shape;
250 : 432 : fd.density = 5.0;
251 : 432 : fd.friction = 0.1;
252 : 432 : fd.filter.categoryBits = 0x0020;
253 : 432 : fd.filter.maskBits = 0x001;
254 : : fd.restitution = 0.0;
255 : :
256 : 432 : hull_ = world_->CreateBody(&bd);
257 : 431 : hull_->CreateFixture(&fd);
258 : 432 : b2Vec2 force = Vec2(RandUniform(-kInitialRandom, kInitialRandom)(*gen), 0);
259 [ + - ]: 432 : hull_->ApplyForceToCenter(force, true);
260 : : }
261 : :
262 : : // leg
263 [ + + ]: 1296 : for (int index = 0; index < 2; ++index) {
264 [ + + ]: 864 : float sign = index == 0 ? -1 : 1;
265 : :
266 : : // upper leg
267 : : b2BodyDef bd;
268 : 864 : bd.type = b2_dynamicBody;
269 : 864 : bd.position = Vec2(init_x, init_y - kLegH / 2 - kLegDown);
270 : 864 : bd.angle = sign * 0.05f;
271 : :
272 : 864 : b2PolygonShape shape;
273 : 864 : shape.SetAsBox(static_cast<float>(kLegW / 2),
274 : 864 : static_cast<float>(kLegH / 2));
275 : :
276 : : b2FixtureDef fd;
277 : 864 : fd.shape = &shape;
278 : 864 : fd.density = 1.0;
279 : 864 : fd.filter.categoryBits = 0x0020;
280 : 864 : fd.filter.maskBits = 0x001;
281 : : fd.restitution = 0.0;
282 : :
283 : 864 : legs_[index * 2] = world_->CreateBody(&bd);
284 : 864 : legs_[index * 2]->CreateFixture(&fd);
285 : 863 : ground_contact_[index * 2] = 0;
286 : :
287 : : b2RevoluteJointDef rjd;
288 : 863 : rjd.bodyA = hull_;
289 : 863 : rjd.bodyB = legs_[index * 2];
290 : 863 : rjd.localAnchorA = Vec2(0, kLegDown);
291 : 864 : rjd.localAnchorB = Vec2(0, kLegH / 2);
292 : 864 : rjd.referenceAngle = rjd.bodyB->GetAngle() - rjd.bodyA->GetAngle();
293 : 864 : rjd.enableMotor = true;
294 : 864 : rjd.enableLimit = true;
295 : 864 : rjd.maxMotorTorque = kMotorsTorque;
296 : 864 : rjd.motorSpeed = sign;
297 : 864 : rjd.lowerAngle = -0.8;
298 : 864 : rjd.upperAngle = 1.1;
299 : 864 : rjd.type = b2JointType::e_revoluteJoint;
300 : 863 : joints_[index * 2] =
301 : 864 : static_cast<b2RevoluteJoint*>(world_->CreateJoint(&rjd));
302 : :
303 : : // lower leg
304 : 863 : bd.position = Vec2(init_x, init_y - kLegH * 3 / 2 - kLegDown);
305 : 863 : shape.SetAsBox(static_cast<float>(0.8 * kLegW / 2),
306 : 863 : static_cast<float>(kLegH / 2));
307 : 864 : legs_[index * 2 + 1] = world_->CreateBody(&bd);
308 : 863 : legs_[index * 2 + 1]->CreateFixture(&fd);
309 : 864 : ground_contact_[index * 2 + 1] = 0;
310 : :
311 : 864 : rjd.bodyA = legs_[index * 2];
312 : 864 : rjd.bodyB = legs_[index * 2 + 1];
313 : 864 : rjd.localAnchorA = Vec2(0, -kLegH / 2);
314 : 864 : rjd.localAnchorB = Vec2(0, kLegH / 2);
315 : 864 : rjd.referenceAngle = rjd.bodyB->GetAngle() - rjd.bodyA->GetAngle();
316 : 864 : rjd.motorSpeed = 1;
317 : 864 : rjd.lowerAngle = -1.6;
318 : 864 : rjd.upperAngle = -0.1;
319 : 864 : joints_[index * 2 + 1] =
320 : 864 : static_cast<b2RevoluteJoint*>(world_->CreateJoint(&rjd));
321 : : }
322 : 432 : }
323 : :
324 : 266874 : void BipedalWalkerBox2dEnv::StepBox2d(std::mt19937* gen, float action0,
325 : : float action1, float action2,
326 : : float action3) {
327 [ - + ]: 266874 : float clip0 = std::min(std::max(std::abs(action0), 0.0f), 1.0f);
328 [ - + ]: 266874 : float clip1 = std::min(std::max(std::abs(action1), 0.0f), 1.0f);
329 [ - + ]: 266874 : float clip2 = std::min(std::max(std::abs(action2), 0.0f), 1.0f);
330 [ - + ]: 266874 : float clip3 = std::min(std::max(std::abs(action3), 0.0f), 1.0f);
331 : 266874 : joints_[0]->SetMotorSpeed(kSpeedHip * Sign(action0));
332 : 266986 : joints_[1]->SetMotorSpeed(kSpeedKnee * Sign(action1));
333 : 266840 : joints_[2]->SetMotorSpeed(kSpeedHip * Sign(action2));
334 : 266818 : joints_[3]->SetMotorSpeed(kSpeedKnee * Sign(action3));
335 : 266797 : joints_[0]->SetMaxMotorTorque(kMotorsTorque * clip0);
336 : 266732 : joints_[1]->SetMaxMotorTorque(kMotorsTorque * clip1);
337 : 266658 : joints_[2]->SetMaxMotorTorque(kMotorsTorque * clip2);
338 : 266601 : joints_[3]->SetMaxMotorTorque(kMotorsTorque * clip3);
339 : :
340 : 266378 : world_->Step(static_cast<float>(1.0 / kFPS), 6 * 30, 2 * 30);
341 : :
342 : 267206 : auto pos = hull_->GetPosition();
343 : 267206 : auto vel = hull_->GetLinearVelocity();
344 : :
345 [ + + ]: 2937496 : for (int i = 0; i < kLidarNum; ++i) {
346 : 2670303 : lidar_[i].fraction = 1.0;
347 : 2669940 : world_->RayCast(&lidar_[i], pos,
348 : 2670290 : Vec2(pos.x + std::sin(1.5 * i / 10.0) * kLidarRange,
349 : 2670303 : pos.y - std::cos(1.5 * i / 10.0) * kLidarRange));
350 : : }
351 : :
352 : 267193 : obs_[0] = hull_->GetAngle();
353 : 267193 : obs_[1] = 2.0 * hull_->GetAngularVelocity() / kFPS;
354 : 267193 : obs_[2] = 0.3 * vel.x * kViewportW / kScaleDouble / kFPS;
355 : 267193 : obs_[3] = 0.3 * vel.y * kViewportH / kScaleDouble / kFPS;
356 : 267193 : obs_[4] = joints_[0]->GetJointAngle();
357 : 267173 : obs_[5] = joints_[0]->GetJointSpeed() / kSpeedHip;
358 : 267169 : obs_[6] = joints_[1]->GetJointAngle() + 1.0;
359 : 267156 : obs_[7] = joints_[1]->GetJointSpeed() / kSpeedKnee;
360 : 267155 : obs_[8] = ground_contact_[1];
361 : 267155 : obs_[9] = joints_[2]->GetJointAngle();
362 : 267144 : obs_[10] = joints_[2]->GetJointSpeed() / kSpeedHip;
363 : 267146 : obs_[11] = joints_[3]->GetJointAngle() + 1.0;
364 : 267155 : obs_[12] = joints_[3]->GetJointSpeed() / kSpeedKnee;
365 : 267118 : obs_[13] = ground_contact_[3];
366 [ + + ]: 2938608 : for (int i = 0; i < kLidarNum; ++i) {
367 : 2671490 : obs_[14 + i] = lidar_[i].fraction;
368 : : }
369 : :
370 [ + + ]: 267118 : auto shaping = 130 * pos.x / kScaleFloat - 5 * std::abs(obs_[0]);
371 : 267118 : reward_ = 0;
372 [ + + ]: 267118 : if (elapsed_step_ > 0) {
373 : 266684 : reward_ = shaping - prev_shaping_;
374 : : }
375 : 267118 : prev_shaping_ = shaping;
376 : 267118 : reward_ -= 0.00035f * kMotorsTorque * (clip0 + clip1 + clip2 + clip3);
377 : :
378 [ + + + + ]: 267118 : if (done_ || pos.x < 0) {
379 : 313 : done_ = true;
380 : 313 : reward_ = -100;
381 : : }
382 [ + + ]: 267118 : if (pos.x > (kTerrainLength - kTerrainGrass) * kTerrainStep) {
383 : 18 : done_ = true;
384 : : }
385 [ + + ]: 267118 : if (elapsed_step_ >= max_episode_steps_) {
386 : 65 : done_ = true;
387 : : }
388 : :
389 [ + + ]: 267118 : scroll_ = pos.x - static_cast<float>(kViewportW / kScaleDouble / 5);
390 : : // info
391 : : terrain_edge_path2_.clear();
392 : : terrain_poly_path4_.clear();
393 : : leg_path4_.clear();
394 : : hull_path5_.clear();
395 : : // terrain
396 [ + + + + ]: 55786813 : for (auto* b : terrain_) {
397 : : b2Fixture& f = b->GetFixtureList()[0];
398 [ + + ]: 55519616 : auto trans = f.GetBody()->GetTransform();
399 [ + + ]: 55519616 : if (f.GetShape()->GetType() == 1) {
400 : : auto* shape = static_cast<b2EdgeShape*>(f.GetShape());
401 : 52630461 : b2Vec2 v1 = kScaleFloat * b2Mul(trans, shape->m_vertex1);
402 : 52630461 : b2Vec2 v2 = kScaleFloat * b2Mul(trans, shape->m_vertex2);
403 : 52630461 : terrain_edge_path2_.emplace_back(v1.x);
404 : 52548624 : terrain_edge_path2_.emplace_back(v1.y);
405 : 52580957 : terrain_edge_path2_.emplace_back(v2.x);
406 : 52620985 : terrain_edge_path2_.emplace_back(v2.y);
407 : : } else {
408 : : auto* shape = static_cast<b2PolygonShape*>(f.GetShape());
409 [ + + ]: 14819169 : for (int i = 0; i < shape->m_count; ++i) {
410 : 11893271 : b2Vec2 v = kScaleFloat * b2Mul(trans, shape->m_vertices[i]);
411 : 11893271 : terrain_poly_path4_.emplace_back(v.x);
412 : 11889803 : terrain_poly_path4_.emplace_back(v.y);
413 : : }
414 : : }
415 : : }
416 : : // legs
417 [ + + ]: 1335561 : for (auto* b : legs_) {
418 : : b2Fixture& f = b->GetFixtureList()[0];
419 : 1068373 : auto trans = f.GetBody()->GetTransform();
420 : : auto* shape = static_cast<b2PolygonShape*>(f.GetShape());
421 [ + + ]: 5338348 : for (int i = 0; i < shape->m_count; ++i) {
422 [ + - ]: 4269984 : b2Vec2 v = kScaleFloat * b2Mul(trans, shape->m_vertices[i]);
423 [ + - ]: 4269984 : leg_path4_.emplace_back(v.x);
424 [ + - ]: 4269528 : leg_path4_.emplace_back(v.y);
425 : : }
426 : : }
427 : : // hull
428 : 267188 : b2Fixture& f = hull_->GetFixtureList()[0];
429 : 267188 : auto trans = f.GetBody()->GetTransform();
430 : : auto* shape = static_cast<b2PolygonShape*>(f.GetShape());
431 [ + + ]: 1602721 : for (int i = 0; i < shape->m_count; ++i) {
432 : 1335563 : b2Vec2 v = kScaleFloat * b2Mul(trans, shape->m_vertices[i]);
433 : 1335563 : hull_path5_.emplace_back(v.x);
434 : 1335463 : hull_path5_.emplace_back(v.y);
435 : : }
436 : 267158 : }
437 : :
438 : 432 : void BipedalWalkerBox2dEnv::BipedalWalkerReset(std::mt19937* gen) {
439 : 432 : elapsed_step_ = 0;
440 : 432 : done_ = false;
441 : 432 : ResetBox2d(gen);
442 : 432 : StepBox2d(gen, 0, 0, 0, 0);
443 : 432 : }
444 : :
445 : 266626 : void BipedalWalkerBox2dEnv::BipedalWalkerStep(std::mt19937* gen, float action0,
446 : : float action1, float action2,
447 : : float action3) {
448 : 266626 : ++elapsed_step_;
449 : 266626 : StepBox2d(gen, action0, action1, action2, action3);
450 : 266728 : }
451 : :
452 : 29 : void BipedalWalkerBox2dEnv::Render(int width, int height, int /*camera_id*/,
453 : : unsigned char* rgb) {
454 [ - + ]: 29 : if (hull_ == nullptr) {
455 [ # # ]: 0 : throw std::runtime_error("render called before BipedalWalker reset");
456 : : }
457 : :
458 : : auto to_point = [](float x, float y) {
459 [ + - + - : 7396 : return cv::Point(static_cast<int>(std::lround(x)),
+ - + - +
- + - ]
460 : : static_cast<int>(std::lround(y)));
461 : : };
462 : :
463 [ - + ]: 29 : const auto scroll_px = static_cast<int>(std::lround(scroll_ * kScaleFloat));
464 : 29 : const auto viewport_w = static_cast<int>(kViewportW);
465 : 29 : const auto viewport_h = static_cast<int>(kViewportH);
466 [ - + + - ]: 58 : int surf_width = std::max(viewport_w, viewport_w + std::max(scroll_px, 0));
467 : :
468 : 29 : cv::Mat surf(viewport_h, surf_width, CV_8UC3, cv::Scalar(255, 215, 215));
469 : :
470 : : std::vector<cv::Point> background = {
471 [ + - ]: 29 : to_point(static_cast<float>(scroll_px), 0.0f),
472 : 29 : to_point(static_cast<float>(scroll_px + viewport_w), 0.0f),
473 : 29 : to_point(static_cast<float>(scroll_px + viewport_w),
474 : : static_cast<float>(viewport_h)),
475 : 29 : to_point(static_cast<float>(scroll_px), static_cast<float>(viewport_h)),
476 [ + - ]: 29 : };
477 [ + - + - ]: 29 : cv::fillConvexPoly(surf, background, cv::Scalar(255, 215, 215));
478 : :
479 [ + + ]: 319 : for (const auto& cloud : cloud_poly_) {
480 [ - + ]: 290 : if (cloud.x2 < scroll_ / 2) {
481 : 255 : continue;
482 : : }
483 : 545 : if (cloud.x1 >
484 [ + + ]: 290 : scroll_ / 2 + static_cast<float>(kViewportW / kScaleDouble)) {
485 : 255 : continue;
486 : : }
487 : : std::vector<cv::Point> polygon;
488 [ + - ]: 35 : polygon.reserve(cloud.points.size());
489 [ + + ]: 210 : for (const auto& point : cloud.points) {
490 : 175 : polygon.push_back(to_point(point.x * kScaleFloat + scroll_px / 2.0f,
491 [ + - ]: 175 : point.y * kScaleFloat));
492 : : }
493 [ + - + - ]: 35 : cv::fillConvexPoly(surf, polygon, cv::Scalar(255, 255, 255));
494 [ + - + - ]: 35 : cv::polylines(surf, polygon, true, cv::Scalar(255, 255, 255), 1,
495 : : cv::LINE_AA);
496 : 35 : }
497 : :
498 [ + + ]: 5800 : for (std::size_t i = 0; i + 3 < terrain_edge_path2_.size(); i += 4) {
499 : : std::vector<cv::Point> polygon = {
500 [ + - ]: 5771 : to_point(terrain_edge_path2_[i], terrain_edge_path2_[i + 1]),
501 : 5771 : to_point(terrain_edge_path2_[i + 2], terrain_edge_path2_[i + 3]),
502 : 5771 : to_point(terrain_edge_path2_[i + 2], 0.0f),
503 : 5771 : to_point(terrain_edge_path2_[i], 0.0f),
504 [ + - ]: 5771 : };
505 [ + - + - ]: 5771 : cv::fillConvexPoly(surf, polygon, cv::Scalar(76, 153, 102));
506 [ + - ]: 5771 : cv::line(surf, to_point(terrain_edge_path2_[i], terrain_edge_path2_[i + 1]),
507 : : to_point(terrain_edge_path2_[i + 2], terrain_edge_path2_[i + 3]),
508 : 5771 : cv::Scalar(0, 255, 0), 1, cv::LINE_AA);
509 : 5771 : }
510 : :
511 [ + + ]: 232 : for (std::size_t i = 0; i + 7 < terrain_poly_path4_.size(); i += 8) {
512 : : std::vector<cv::Point> polygon;
513 [ + - ]: 203 : polygon.reserve(4);
514 [ + + ]: 1015 : for (std::size_t j = 0; j < 8; j += 2) {
515 : : polygon.push_back(
516 [ + - ]: 812 : to_point(terrain_poly_path4_[i + j], terrain_poly_path4_[i + j + 1]));
517 : : }
518 [ + - + - ]: 203 : cv::fillConvexPoly(surf, polygon, cv::Scalar(255, 255, 255));
519 [ + - + - ]: 203 : cv::polylines(surf, polygon, true, cv::Scalar(153, 153, 153), 1,
520 : : cv::LINE_AA);
521 : 203 : }
522 : :
523 : : auto leg_fill_color = [](int sign) {
524 : 116 : return cv::Scalar(127 - sign * 25, 76 - sign * 25, 153 - sign * 25);
525 : : };
526 : : auto leg_outline_color = [](int sign) {
527 : 116 : return cv::Scalar(76 - sign * 25, 51 - sign * 25, 102 - sign * 25);
528 : : };
529 [ + + ]: 145 : for (std::size_t i = 0; i + 7 < leg_path4_.size(); i += 8) {
530 : : std::vector<cv::Point> polygon;
531 [ + - ]: 116 : polygon.reserve(4);
532 [ + + ]: 580 : for (std::size_t j = 0; j < 8; j += 2) {
533 [ + - ]: 464 : polygon.push_back(to_point(leg_path4_[i + j], leg_path4_[i + j + 1]));
534 : : }
535 : 116 : const auto leg_index = static_cast<int>(i / 8);
536 [ + + ]: 116 : int sign = leg_index < 2 ? -1 : 1;
537 [ + - + - ]: 116 : cv::fillConvexPoly(surf, polygon, leg_fill_color(sign));
538 [ + - + - ]: 116 : cv::polylines(surf, polygon, true, leg_outline_color(sign), 1, cv::LINE_AA);
539 : 116 : }
540 : :
541 [ + - ]: 29 : if (!hull_path5_.empty()) {
542 : : std::vector<cv::Point> hull;
543 [ + - ]: 29 : hull.reserve(hull_path5_.size() / 2);
544 [ + + ]: 174 : for (std::size_t i = 0; i + 1 < hull_path5_.size(); i += 2) {
545 [ + - ]: 145 : hull.push_back(to_point(hull_path5_[i], hull_path5_[i + 1]));
546 : : }
547 [ + - + - ]: 29 : cv::fillConvexPoly(surf, hull, cv::Scalar(229, 51, 127));
548 [ + - + - ]: 29 : cv::polylines(surf, hull, true, cv::Scalar(127, 76, 76), 1, cv::LINE_AA);
549 : 29 : }
550 : :
551 : 29 : auto flag_x = static_cast<float>(kTerrainStep * 3 * kScaleFloat);
552 : 29 : auto flag_y1 = static_cast<float>(kTerrainHeight * kScaleFloat);
553 : 29 : float flag_y2 = flag_y1 + 50.0f;
554 [ + - ]: 29 : cv::line(surf, to_point(flag_x, flag_y1), to_point(flag_x, flag_y2),
555 [ + - ]: 29 : cv::Scalar(0, 0, 0), 1, cv::LINE_AA);
556 : : std::vector<cv::Point> flag = {
557 : 29 : to_point(flag_x, flag_y2),
558 : 29 : to_point(flag_x, flag_y2 - 10.0f),
559 : 29 : to_point(flag_x + 25.0f, flag_y2 - 5.0f),
560 [ + - ]: 29 : };
561 [ + - + - ]: 29 : cv::fillConvexPoly(surf, flag, cv::Scalar(0, 51, 230));
562 [ + - + - : 29 : cv::polylines(surf, flag, true, cv::Scalar(0, 0, 0), 1, cv::LINE_AA);
+ - ]
563 : :
564 [ + - ]: 29 : cv::flip(surf, surf, 0);
565 : :
566 [ + - ]: 29 : cv::Mat view(viewport_h, viewport_w, CV_8UC3, cv::Scalar(255, 215, 215));
567 [ - + ]: 29 : int src_x = std::max(scroll_px, 0);
568 [ + - ]: 29 : int dst_x = std::max(-scroll_px, 0);
569 [ - + ]: 29 : int copy_width = std::min(surf.cols - src_x, viewport_w - dst_x);
570 [ + - ]: 29 : if (copy_width > 0) {
571 : 29 : surf(cv::Rect(src_x, 0, copy_width, viewport_h))
572 [ + - ]: 29 : .copyTo(view(cv::Rect(dst_x, 0, copy_width, viewport_h)));
573 : : }
574 : :
575 [ + - ]: 29 : cv::Mat output(height, width, CV_8UC3, rgb);
576 [ + + + - ]: 29 : if (width == viewport_w && height == viewport_h) {
577 [ + - ]: 14 : cv::cvtColor(view, output, cv::COLOR_BGR2RGB);
578 : : return;
579 : : }
580 : 15 : cv::Mat resized;
581 [ + - + - ]: 15 : cv::resize(view, resized, cv::Size(width, height));
582 [ + - ]: 15 : cv::cvtColor(resized, output, cv::COLOR_BGR2RGB);
583 : 29 : }
584 : :
585 : : } // namespace box2d
|
