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
