def testIfSeesPlayer(self, player, world): if not self.initialized: return playerScreenPos = player.getScreenPosition() victimToPlayer = Box2D.b2Vec2((playerScreenPos[0] - self.screenPosition[0],playerScreenPos[1] - self.screenPosition[1])); victimToPlayer.Normalize() rawDot = min(1., max(-1., Box2D.b2Dot(victimToPlayer, (self.direction[0], self.direction[1])))) if (rawDot > 0): dot = constants.rad2deg(math.acos(rawDot)) if (dot < self.fovAngle / 2.): if (player.currentRoom == self.currentRoom): self.seesPlayer()
def updateDrive(self, controlState): desiredSpeed = 0 if controlState & WDC_UP: desiredSpeed = self.maxForwardSpeed if controlState & WDC_DOWN: desiredSpeed = self.maxBackwardSpeed # Current speed in forward direction currentForwardNormal = self.body.GetWorldVector((0,1)) currentSpeed = b2.b2Dot(self.getForwardVelocity(), currentForwardNormal) # Apply necessary force force = 0 if desiredSpeed > currentSpeed: force = self.maxDriveForce elif desiredSpeed < currentSpeed: force = -self.maxDriveForce else: return self.body.ApplyForce(force*currentForwardNormal, self.body.worldCenter, True)
def PreSolve(self, contact, old_manifold): """ This is a critical function when there are many contacts in the world. It should be optimized as much as possible. """ if not (self.settings.drawContactPoints or self.settings.drawContactNormals or self.using_contacts): return elif len(self.points) > self.settings.maxContactPoints: return manifold = contact.manifold if manifold.pointCount == 0: return _, state2 = b2.b2GetPointStates(old_manifold, manifold) if not state2: return worldManifold = contact.worldManifold for i, _ in enumerate(state2): point = worldManifold.points[0] bodyA = contact.fixtureA.body bodyB = contact.fixtureB.body vA = bodyA.GetLinearVelocityFromWorldPoint(point) vB = bodyB.GetLinearVelocityFromWorldPoint(point) approachVelocity = b2.b2Dot(vB - vA, worldManifold.normal) # print("approachVelocity", approachVelocity) if abs(approachVelocity) > 1.0: self.collision = True self.points.append({ 'fixtureA': contact.fixtureA, 'fixtureB': contact.fixtureB, 'bodyA': bodyA, 'bodyB': bodyB, 'position': worldManifold.points[i], 'normal': worldManifold.normal, 'state': state2[i] })
def calculate_forces(self, fixture_pairs): for pair in fixture_pairs: density = pair[0].density has_intersection, intersection_points = self.find_intersection( pair[0], pair[1]) if has_intersection: centroid, area = self.compute_centroids(intersection_points) # apply buoyancy force displaced_mass = pair[0].density * area buoyancy_force = displaced_mass * -self.gravity pair[1].body.ApplyForce(force=buoyancy_force, point=centroid, wake=True) # apply complex drag for i in range(len(intersection_points)): v0 = intersection_points[i] v1 = intersection_points[(i + 1) % len(intersection_points)] mid_point = 0.5 * (v0 + v1) ##### DRAG # find relative velocity between object and fluid at edge midpoint vel_dir = pair[1].body.GetLinearVelocityFromWorldPoint(mid_point) - \ pair[0].body.GetLinearVelocityFromWorldPoint(mid_point) vel = vel_dir.Normalize() edge = v1 - v0 edge_length = edge.Normalize() normal = Box2D.b2Cross(-1, edge) drag_dot = Box2D.b2Dot(normal, vel_dir) if drag_dot >= 0: # normal points backwards - this is not a leading edge # apply drag drag_mag = drag_dot * self.drag_mod * edge_length * density * vel * vel drag_mag = min(drag_mag, self.max_drag) drag_force = drag_mag * -vel_dir pair[1].body.ApplyForce(force=drag_force, point=mid_point, wake=True) # apply lift lift_dot = Box2D.b2Dot(edge, vel_dir) lift_mag = drag_dot * lift_dot * self.lift_mod * edge_length * density * vel * vel lift_mag = min(lift_mag, self.max_lift) lift_dir = Box2D.b2Cross(1, vel_dir) lift_force = lift_mag * lift_dir pair[1].body.ApplyForce(force=lift_force, point=mid_point, wake=True) ##### PUSH # Apply a linear force to an object linked to a rotation joint applying torque. # Torque and angular inertia are used to calculate the magnitude of the linear force body_to_check = pair[1].body # Simplification /!\ # joint = pair[1].body.joints[0].joint # joints_to_check = [joint_edge.joint for joint_edge in body_to_check.joints] joints_to_check = [ joint_edge.joint for joint_edge in body_to_check.joints if joint_edge.joint.bodyB == body_to_check ] for joint in joints_to_check: if joint.lowerLimit < joint.angle < joint.upperLimit: torque = joint.GetMotorTorque(60) # Calculate angular inertia of the object moment_of_inertia = body_to_check.inertia angular_velocity = body_to_check.angularVelocity angular_inertia = moment_of_inertia * angular_velocity # Calculate the force applied to the object world_center = body_to_check.worldCenter anchor = joint.anchorB lever_vector = world_center - anchor # vector from pivot to point of application of the force force_applied_at_center = Box2D.b2Cross( lever_vector, -torque) push_dot = Box2D.b2Dot(normal, force_applied_at_center) if push_dot > 0: vel = torque + angular_inertia push_mag = push_dot * self.push_mod * edge_length * density * vel * vel # Wrong approximation /!\ # push_mag = min(push_mag, self.max_push) push_force = np.clip( push_mag * -force_applied_at_center, -self.max_push, self.max_push) body_to_check.ApplyForce( force=push_force, point=joint. anchorB, #body_to_check.worldCenter, wake=True)
def apply_wheel_resistance(car): currentRightNormal = car.GetWorldVector(Box2D.b2Vec2(0, 1)) latVel = Box2D.b2Dot(currentRightNormal, car.__GetLinearVelocity()) * currentRightNormal impulse = car.__GetMass() * -latVel car.ApplyLinearImpulse(impulse, car.GetWorldPoint((0, 0)), True)
def getForwardVelocity(self): currentForwardNormal = self.body.GetWorldVector((0,1)) return b2.b2Dot(currentForwardNormal, self.body.linearVelocity) * currentForwardNormal
def getLateralVelocity(self): currentRightNormal = self.body.GetWorldVector((1,0)) return b2.b2Dot(currentRightNormal, self.body.linearVelocity) * currentRightNormal