File size: 8,071 Bytes
09a6f7f |
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 |
/**
* @classdesc Class that handles the climbing dynamics.
*/
class ClimbingDynamics {
constructor(){};
/**
* Prepares the agent's sensors to grasp or release according to the actions.
* @param actions {Array} - Actions of the agent
* @param agent_body {Object} - Climber morphology
* @param world - {Object} - Box2D world
*/
before_step_climbing_dynamics(actions, agent_body, world){
for(let i = 0; i < agent_body.sensors.length; i++){
let action_to_check = actions[actions.length - i - 1];
let sensor_to_check = agent_body.sensors[agent_body.sensors.length - i - 1];
if(action_to_check > 0){ // Check whether the sensor should grasp or release
sensor_to_check.GetUserData().ready_to_attach = true;
}
else {
sensor_to_check.GetUserData().ready_to_attach = false;
if(sensor_to_check.GetUserData().has_joint){ // if released and it had a joint => destroys it
sensor_to_check.GetUserData().has_joint = false;
// Gets a list of all the joints of the sensor body
let sensor_joints = [];
let _joint = sensor_to_check.GetJointList();
while(_joint != null){
sensor_joints.push(_joint.joint);
_joint = _joint.next;
}
// Finds the index of the first revolute joint
const isRevolute = (s) => s.m_type == b2.Joint.e_revoluteJoint;
let idx_to_destroy = sensor_joints.findIndex(isRevolute);
if(idx_to_destroy != -1){
world.DestroyJoint(sensor_joints[idx_to_destroy]);
}
}
}
}
}
/**
* Creates joints between sensors ready to grasp if collision with graspable area was detected
* @param contact_detector {Object}
* @param world {Object} - Box2D world
*/
after_step_climbing_dynamics(contact_detector, world){
// Adds climbing joints if needed
for(let i = 0; i < contact_detector.contact_dictionaries.sensors.length; i++){
let sensor = contact_detector.contact_dictionaries.sensors[i];
if(contact_detector.contact_dictionaries.bodies[i].length > 0
&& sensor.GetUserData().ready_to_attach
&& !sensor.GetUserData().has_joint){
let other_bodies = [...contact_detector.contact_dictionaries.bodies[i]];
for(let other_body of other_bodies){
// Checks if still overlapping after solver
// Super coarse yet fast way, mainly useful for creepers
let other_body_shape = other_body.GetFixtureList().GetShape();
let x_values = [];
let y_values = [];
if(other_body_shape.m_type == b2.Shape.e_polygon){
for(let i = 0; i < other_body_shape.m_count; i++) {
x_values.push(other_body.GetWorldPoint(other_body_shape.m_vertices[i]).x);
y_values.push(other_body.GetWorldPoint(other_body_shape.m_vertices[i]).y);
}
}
else if(other_body_shape.m_type == b2.Shape.e_edge){
x_values = [other_body_shape.m_vertex1.x, other_body_shape.m_vertex2.x];
y_values = [other_body_shape.m_vertex1.y, other_body_shape.m_vertex2.y];
}
let radius = sensor.GetFixtureList().GetShape().m_radius + 0.01;
let sensor_world_center = sensor.GetWorldCenter();
if(sensor_world_center.x + radius > Math.min(...x_values)
&& sensor_world_center.x - radius < Math.max(...x_values)
&& sensor_world_center.y + radius > Math.min(...y_values)
&& sensor_world_center.y - radius < Math.max(...y_values)){
let rjd = new b2.RevoluteJointDef();
rjd.Initialize(sensor, other_body, sensor_world_center);
let joint = world.CreateJoint(rjd);
joint.SetUserData(new CustomBodyUserData(false, false, "grip"));
joint.GetBodyA().GetUserData().joint = joint;
sensor.GetUserData().has_joint = true;
break;
}
else {
// Removes other_body from the list of bodies in contact with the sensor
let sensor_idx = contact_detector.contact_dictionaries.sensors.indexOf(sensor);
if(sensor_idx != -1){
let other_idx = contact_detector.contact_dictionaries.bodies[sensor_idx].indexOf(other_body);
contact_detector.contact_dictionaries.bodies[sensor_idx].splice(other_idx, 1);
if(contact_detector.contact_dictionaries.bodies[sensor_idx].length == 0){
sensor.GetUserData().has_contact = false;
}
}
}
}
}
}
}
}
/**
* @classdesc Stores contacts between sensors and graspable surfaces in a dictionaries associated to the sensor.
* @constructor
*/
function ClimbingContactDetector() {
b2.ContactListener.call(this);
this.contact_dictionaries = {
sensors: [],
bodies: []
};
}
ClimbingContactDetector.prototype = Object.create(b2.ContactListener.prototype);
ClimbingContactDetector.prototype.constructor = ClimbingContactDetector;
ClimbingContactDetector.prototype.BeginContact = function (contact) {
let bodies = [contact.GetFixtureA().GetBody(), contact.GetFixtureB().GetBody()];
for(let i = 0; i < bodies.length; i++){
let body = bodies[i];
if(body.GetUserData().object_type == CustomUserDataObjectTypes.BODY_SENSOR
&& body.GetUserData().check_contact){
let other_body = bodies[(i + 1) % 2];
if(other_body.GetUserData().object_type == CustomUserDataObjectTypes.GRIP_TERRAIN
|| other_body.GetUserData().object_type == CustomUserDataObjectTypes.SENSOR_GRIP_TERRAIN){
body.GetUserData().has_contact = true;
let idx = this.contact_dictionaries.sensors.indexOf(body);
if(idx != -1){
this.contact_dictionaries.bodies[idx].push(other_body);
}
else{
this.contact_dictionaries.sensors.push(body);
this.contact_dictionaries.bodies.push([other_body]);
}
}
else{
return;
}
}
}
};
ClimbingContactDetector.prototype.EndContact = function (contact){
let bodies = [contact.GetFixtureA().GetBody(), contact.GetFixtureB().GetBody()];
for(let i = 0; i < bodies.length; i++) {
let body = bodies[i];
let other_body = bodies[(i + 1) % 2];
if(body.GetUserData().object_type == CustomUserDataObjectTypes.BODY_SENSOR &&
body.GetUserData().check_contact && body.GetUserData().has_contact){
let body_idx = this.contact_dictionaries.sensors.indexOf(body);
if (body_idx != -1) {
let other_idx = this.contact_dictionaries.bodies[body_idx].indexOf(other_body);
if(other_idx != -1){
this.contact_dictionaries.bodies[body_idx].splice(other_idx, 1);
}
if(this.contact_dictionaries.bodies[body_idx].length == 0){
body.GetUserData().has_contact = false;
}
}
}
}
};
ClimbingContactDetector.prototype.Reset = function(){
this.contact_dictionaries = {
body: []
};
}; |