def sweep (self):
self.page ('traction')
self.group ('wheels', True)
self.velocity.set (self.velocity + self.acceleration * sp.world.period, self.velocity < self.targetVelocity, self.velocity - self.acceleration * sp.world.period)
self.midWheelAngularVelocity.set (self.velocity / pm.displacementPerWheelAngle)
self.midWheelAngle.set (self.midWheelAngle + self.midWheelAngularVelocity * sp.world.period)
self.tangentialVelocity.set (self.midWheelAngularVelocity * pm.displacementPerWheelAngle)
self.midSteeringAngle.set (sp.atan (0.5 * sp.tan (self.steeringAngle)))
self.inverseMidCurveRadius.set (sp.sin (self.midSteeringAngle) / pm.wheelShift)
self.midAngularVelocity.set (sp.degreesPerRadian * self.tangentialVelocity * self.inverseMidCurveRadius)
self.attitudeAngle.set (self.attitudeAngle + self.midAngularVelocity * sp.world.period)
self.courseAngle.set (self.attitudeAngle + self.midSteeringAngle)
self.radialAcceleration.set (sp.max (abs (self.tangentialVelocity * self.tangentialVelocity * self.inverseMidCurveRadius) - 0.5, 0))
self.slipping.mark (sp.abs (self.radialAcceleration) > 0.55)
self.radialVelocity.set (self.radialVelocity + self.radialAcceleration * sp.world.period, self.slipping, 0)
self.velocityX.set (self.tangentialVelocity * sp.cos (self.courseAngle) + self.radialVelocity * sp.sin (self.courseAngle))
self.velocityY.set (self.tangentialVelocity * sp.sin (self.courseAngle) + self.radialVelocity * sp.cos (self.courseAngle))
self.positionX.set (self.positionX + self.velocityX * sp.world.period)
self.positionY.set (self.positionY + self.velocityY * sp.world.period)
def getXyzAngles(rotMat): # rotMat == rotMatZ @ rotMatY @ rotMatX
# Source: Computing Euler angles from a rotation matrix, by Gregory G. Slabaugh
# http://thomasbeatty.com/MATH%20PAGES/ARCHIVES%20-%20NOTES/Applied%20Math/euler%20angles.pdf
angleZ = 0
if isClose(rotMat[2, 0], -1):
angleY = sp.pi / 2.0
angleX = sp.atan2(rotMat[0, 1], rotMat[0, 2])
elif isClose(rotMat[2, 0], 1):
angleY = -sp.pi / 2
angleX = sp.atan2(-rotMat[0, 1], -rotMat[0, 2])
else:
angleY = -sp.asin(rotMat[2, 0])
cosAngleY = sp.cos(angleY)
angleX = sp.atan2(rotMat[2, 1] / cosAngleY, rotMat[2, 2] / cosAngleY)
angleZ = sp.atan2(rotMat[1, 0] / cosAngleY, rotMat[0, 0] / cosAngleY)
return np.array([angleX, angleY, angleZ])
def sweep(self):
self.local_sail_angle.set(self.local_sail_angle - 1, self.local_sail_angle > self.target_sail_angle)
self.local_sail_angle.set(self.local_sail_angle + 1, self.local_sail_angle < self.target_sail_angle)
self.global_sail_angle.set((self.sailboat_rotation + self.local_sail_angle + 180) % 360)
self.gimbal_rudder_angle.set(self.gimbal_rudder_angle - 1,
self.gimbal_rudder_angle > self.target_gimbal_rudder_angle)
self.gimbal_rudder_angle.set(self.gimbal_rudder_angle + 1,
self.gimbal_rudder_angle < self.target_gimbal_rudder_angle)
if not self.pause:
# Calculate forward force in N based on the angle between the sail and the wind
self.sail_alpha.set(sp.abs(self.global_sail_angle - sp.world.wind.wind_direction) % 360)
self.sail_alpha.set(sp.abs(180 - self.sail_alpha) % 360, self.sail_alpha > 90)
self.boot_wind.set(self.forward_velocity)
self.alfa.set(sp.world.wind.wind_direction - self.sailboat_rotation)
# headforce = sail area * wind pressure
# wind pressure = 0.613 * wind speed ^ 2
self.head_wind_force.set(2 * 0.613 * 2 * self.forward_velocity)
#bereken de kracht van de apparent wind door de echte wind te gebruiken en de rotatie van de boot
self.apparent_wind.set(sp.sqrt(sp.world.wind.wind_scalar * sp.world.wind.wind_scalar + self.head_wind_force * self.head_wind_force + 2 * sp.world.wind.wind_scalar * self.head_wind_force * sp.cos(self.alfa)))
#bereken de hoek van de apparent wind aan de hand van de echte wind en de rotatie van de boot
self.apparent_wind_angle.set(sp.acos(round((sp.world.wind.wind_scalar * sp.cos(self.alfa) + self.head_wind_force) / self.apparent_wind, 2)))
# print(round((sp.world.wind.wind_scalar * sp.cos(self.alfa) + self.forward_velocity) / self.apparent_wind, 2))
self.perpendicular_sail_force.set(self.apparent_wind * sp.sin(self.sail_alpha))
self.forward_sail_force.set(self.perpendicular_sail_force * sp.sin(self.local_sail_angle))
self.forward_sail_force.set(sp.abs(self.forward_sail_force))
self.drag.set((0.83 * self.forward_velocity - 0.38))
if self.drag < 0:
self.drag.set(0)
# Sailing against wind
min_threshold = (self.global_sail_angle - 180) % 360
max_threshold = (self.global_sail_angle + 180) % 360
self.forward_sail_force.set(self.forward_sail_force,
is_sailing_against_wind(min_threshold,
max_threshold,
self.local_sail_angle,
self.global_sail_angle,
sp.world.wind.wind_direction))
# Newton's second law
self.drag_deacceleration.set(self.drag / self.mass)
self.acceleration.set((self.forward_sail_force / self.mass) - self.drag_deacceleration)
self.forward_velocity.set(sp.limit(self.forward_velocity + self.acceleration * sp.world.period, 20))
# Splitting forward velocity vector into vertical and horizontal components
self.vertical_velocity.set(sp.sin(self.sailboat_rotation) * self.forward_velocity)
self.horizontal_velocity.set(sp.cos(self.sailboat_rotation) * self.forward_velocity)
self.position_x.set(self.position_x + self.horizontal_velocity * 0.001)
self.position_y.set(self.position_y + self.vertical_velocity * 0.001)
self.rotation_speed.set(0.001 * self.gimbal_rudder_angle * self.forward_velocity)
self.sailboat_rotation.set((self.sailboat_rotation - self.rotation_speed) % 360)
self.boot_direction.set(180 + self.sailboat_rotation)
self.head_wind_direction.set((self.boot_direction + 180) % 360)
if sp.world.wind.wind_direction <= self.head_wind_direction:
self.true_apparent_wind.set((self.head_wind_direction - self.apparent_wind_angle) % 360)
else:
self.true_apparent_wind.set((self.head_wind_direction + self.apparent_wind_angle) % 360)
def sweep(self):
self.local_sail_angle.set(
self.local_sail_angle - 1,
self.local_sail_angle > self.target_sail_angle)
self.local_sail_angle.set(
self.local_sail_angle + 1,
self.local_sail_angle < self.target_sail_angle)
self.global_sail_angle.set(
(self.sailboat_rotation + self.local_sail_angle + 180) % 360)
self.gimbal_rudder_angle.set(
self.gimbal_rudder_angle - 1,
self.gimbal_rudder_angle > self.target_gimbal_rudder_angle)
self.gimbal_rudder_angle.set(
self.gimbal_rudder_angle + 1,
self.gimbal_rudder_angle < self.target_gimbal_rudder_angle)
if not self.pause:
# Calculate forward force in N based on the angle between the sail and the wind
self.sail_alpha.set(
sp.abs(self.global_sail_angle - sp.world.wind.wind_direction) %
360)
self.sail_alpha.set(
sp.abs(180 - self.sail_alpha) % 360, self.sail_alpha > 90)
self.perpendicular_sail_force.set(sp.world.wind.wind_scalar *
sp.sin(self.sail_alpha))
self.forward_sail_force.set(self.perpendicular_sail_force *
sp.sin(self.local_sail_angle))
self.forward_sail_force.set(sp.abs(self.forward_sail_force))
# Sailing against wind
min_threshold = (self.global_sail_angle - 180) % 360
max_threshold = (self.global_sail_angle + 180) % 360
self.forward_sail_force.set(
0,
is_sailing_against_wind(min_threshold, max_threshold,
self.local_sail_angle,
self.global_sail_angle,
sp.world.wind.wind_direction))
# Newton's second law
self.drag.set(self.forward_velocity * 0.05)
self.acceleration.set(self.forward_sail_force / self.mass -
self.drag)
self.forward_velocity.set(
sp.limit(
self.forward_velocity +
self.acceleration * sp.world.period, 8))
# Splitting forward velocity vector into vertical and horizontal components
self.vertical_velocity.set(
sp.sin(self.sailboat_rotation) * self.forward_velocity)
self.horizontal_velocity.set(
sp.cos(self.sailboat_rotation) * self.forward_velocity)
self.position_x.set(self.position_x +
self.horizontal_velocity * 0.001)
self.position_y.set(self.position_y +
self.vertical_velocity * 0.001)
self.rotation_speed.set(0.001 * self.gimbal_rudder_angle *
self.forward_velocity)
self.sailboat_rotation.set(
(self.sailboat_rotation - self.rotation_speed) % 360)
def getRotZMat(angleZ):
c = sp.cos(angleZ)
s = sp.sin(angleZ)
return np.array([[c, -s, 0], [s, c, 0], [0, 0, 1]])
def getRotYMat(angleY):
c = sp.cos(angleY)
s = sp.sin(angleY)
return np.array([[c, 0, s], [0, 1, 0], [-s, 0, c]])
def getRotXMat(angleX):
c = sp.cos(angleX)
s = sp.sin(angleX)
return np.array([[1, 0, 0], [0, c, -s], [0, s, c]])