trunk/src/breve/CeldasMultiSensor.tz

   1 @use PhysicalControl.
   2 @use Shape.
   3 @use Stationary.
   4 @use Link.
   5 @use MultiBody.
   6
   7 PhysicalControl : CeldasControl {
   8         % This class is used for building simple vehicle
   9         % simulations.  To create a vehicle simulation,
  10         % subclass CeldasControl and use the init method to
  11         % create OBJECT(CeldasObstacle) and
  12         % OBJECT(CeldasVehicle) objects.
  13
  14         + variables:
  15                 floor (object).
  16                 floorShape (object).
  17                 cloudTexture (object).
  18
  19         + to init:
  20                 self enable-lighting.
  21                 #self enable-smooth-drawing.
  22
  23                 floorShape = new Shape.
  24                 floorShape init-with-cube size (200, .2, 200).
  25
  26                 floor = new Stationary.
  27                 floor register with-shape floorShape at-location (0, 0, 0).
  28                 #floor catch-shadows.
  29
  30                 self point-camera at (0, 0, 0) from (3, 3, 24).
  31
  32                 #self enable-shadows.
  33                 #self enable-reflections.
  34
  35                 cloudTexture = (new Image load from "images/clouds.png").
  36                 self set-background-color to (.4, .6, .9).
  37                 self set-background-texture-image to cloudTexture.
  38 }
  39
  40 MultiBody : CeldasLightVehicle (aka CeldasLightVehicles) {
  41         % This object is used in conjunction with OBJECT(CeldasControl) to
  42         % create simple vehicles.
  43
  44         + variables:
  45                 bodyShape (object).
  46                 wheelShape (object).
  47                 sensorShape (object).
  48                 bodyLink (object).
  49
  50         + to init:
  51                 bodyShape = new Shape.
  52                 bodyShape init-with-cube size (4.0, .75, 3.0).
  53
  54                 wheelShape = new Shape.
  55                 wheelShape init-with-polygon-disk radius ( self get-wheel-radius ) sides 20 height ( self get-wheel-width ).
  56                 # 40
  57
  58                 sensorShape = new Shape.
  59                 sensorShape init-with-polygon-cone radius .2 sides 5 height .5.
  60                 # 10
  61
  62                 bodyShape set-density to ( self get-density ).
  63                 bodyLink = new Link.
  64                 bodyLink set-shape to bodyShape.
  65                 bodyLink set-mu to -1.0.
  66                 bodyLink set-eT to .8.
  67
  68                 self set-root to bodyLink.
  69
  70                 self move to (0, 0.9, 0).
  71                 self set-texture-scale to 1.5.
  72
  73         - to get-density:
  74                 return 1.0.
  75
  76         - to get-wheel-width:
  77                 return 0.1.
  78
  79         - to get-wheel-radius:
  80                 return 0.6.
  81
  82         + section "Adding Wheels and Sensors to a Vehicle"
  83
  84         + to add-wheel at location (vector):
  85                 % Adds a wheel at location on the vehicle.  This method returns
  86                 % the wheel which is created, a OBJECT(CeldasWheel).
  87
  88                 wheel, joint (object).
  89
  90                 wheel = new CeldasWheel.
  91                 wheel set-shape to wheelShape.
  92
  93                 joint = new RevoluteJoint.
  94
  95                 joint set-relative-rotation around-axis (1, 0, 0) by 1.5708.
  96                 joint link parent bodyLink to-child wheel with-normal (0, 0, 1)
  97                                         with-parent-point location with-child-point (0, 0, 0).
  98
  99                 wheel set-eT to .8.
 100                 wheel set-texture to 0.
 101                 wheel set-joint to joint.
 102                 joint set-strength-limit to (joint get-strength-hard-limit) / 2.
 103                 wheel set-color to (.6, .6, .6).
 104                 wheel set-mu to 100000.
 105
 106                 self add-dependency on joint.
 107                 self add-dependency on wheel.
 108
 109                 return wheel.
 110
 111         + to add-sensor at location (vector) with-rotation angle = -1.57 (float)
 112                         arround-axis axis = (0, 0, 1) (vector)
 113                         with-normal normal = (1, 0, 0) (vector):
 114                 % Adds a sensor at location on the vehicle.  This method returns
 115                 % the sensor which is created, a OBJECT(CeldasSensor).
 116
 117                 sensor, joint (object).
 118
 119                 sensor = new CeldasSensor.
 120                 sensor set-shape to sensorShape.
 121
 122                 joint = new RevoluteJoint.
 123
 124                 joint set-relative-rotation around-axis axis by angle.
 125                 joint link parent bodyLink to-child sensor with-normal normal
 126                                         with-parent-point location with-child-point (0, 0, 0).
 127
 128                 joint set-double-spring with-strength 300 with-max 0.01 with-min -0.01.
 129
 130                 self add-dependency on joint.
 131                 self add-dependency on sensor.
 132
 133                 sensor set-color to (0, 0, 0).
 134
 135                 return sensor.
 136
 137         + to destroy:
 138                 free sensorShape.
 139                 free wheelShape.
 140                 free bodyShape.
 141
 142                 super destroy.
 143 }
 144
 145 CeldasLightVehicle : CeldasVehicle (aka CeldasVehicles) {
 146         % A heavy duty version of OBJECT(CeldasLightVehicle), this
 147         % vehicle is heavier and harder to control, but more stable
 148         % at higher speeds.
 149
 150         - to get-density:
 151                 return 20.0.
 152
 153         - to get-wheel-width:
 154                 return 0.4.
 155
 156         - to get-wheel-radius:
 157                 return 0.8.
 158 }
 159
 160 Stationary : CeldasObstacle (aka CeldasObstacles) {
 161         % A CeldasObstacle is used in conjunction with OBJECT(CeldasControl)
 162         % and OBJECT(CeldasVehicle).  It is what the OBJECT(CeldasSensor)
 163         % objects on the CeldasVehicle detect.
 164         % <p>
 165         % There are no special behaviors associated with the walls--they're
 166         % basically just plain OBJECT(Stationary) objects.
 167
 168         + to init with-size theSize = (10, 3, .1) (vector) with-color theColor = (1, 0, 0) (vector) at-location theLocation = (0, 0, 0) (vector) with-rotation theRotation = [ ( 0, 0, 1 ), ( 0, 1, 0 ), ( 1, 0, 0 ) ] (matrix):
 169                 self init-with-shape shape (new Shape init-with-cube size theSize) color theColor at-location theLocation with-rotation theRotation.
 170
 171         + to init-with-shape shape theShape (object) color theColor = (1, 0, 0) (vector) at-location theLocation = (0, 0, 0) (vector) with-rotation theRotation = [ ( 1, 0, 0 ), ( 0, 1, 0 ), ( 0, 0, 1 ) ] (matrix):
 172                 self register with-shape theShape at-location theLocation with-rotation theRotation.
 173                 self set-color to theColor.
 174 }
 175
 176 Link : CeldasWheel (aka CeldasWheels) {
 177         % A CeldasWheel is used in conjunction with OBJECT(CeldasVehicle)
 178         % to build Celdas vehicles.  This class is typically not instantiated
 179         % manually, since OBJECT(CeldasVehicle) creates one for you when you
 180         % add a wheel to the vehicle.
 181
 182         + variables:
 183                 joint (object).
 184
 185         - to set-joint to j (object):
 186                 % Used internally.
 187
 188                 joint = j.
 189
 190         + section "Configuring the Wheel's Velocity"
 191
 192         + to set-velocity to n (float):
 193                 % Sets the velocity of this wheel.
 194
 195                 joint set-joint-velocity to n.
 196
 197         + to get-velocity:
 198                 % Gets the velocity of this wheel.
 199
 200                 return (joint get-joint-velocity).
 201
 202 }
 203
 204 Link : CeldasSensor (aka CeldasSensors) {
 205         % A CeldasSensor is used in conjunction with OBJECT(CeldasVehicle)
 206         % to build Celdas vehicles.  This class is typically not instantiated
 207         % manually, since OBJECT(CeldasVehicle) creates one for you when you
 208         % add a sensor to the vehicle.
 209
 210         + variables:
 211                 direction (vector).
 212                 sensorAngle (float).
 213                 value (float).
 214
 215         + to init with-direction theDirection = (0, 1, 0) (vector)
 216                         with-angle theAngle = 1.6 (float):
 217                 direction = theDirection.
 218                 sensorAngle = theAngle.
 219                 value = 0.0.
 220
 221   + section "Configuring the Sensor Values"
 222
 223         + to set-sensor-angle to n (float):
 224                 % Sets the angle in which this sensor can detect obstacles.  The default
 225                 % value of 1.6 means that the sensor can see most of everything in
 226                 % front of it.  Setting the value to be any higher leads to general
 227                 % wackiness, so I don't suggest it.
 228
 229                 sensorAngle = n.
 230
 231   + section "Getting the Sensor Values"
 232
 233         + to get-sensor-value:
 234                 % Gets the sensor value. This should be used from post-iterate,
 235                 % if not, the sensor reading correspond to the previous
 236                 % iteration.
 237
 238                 return value.
 239
 240         + to iterate:
 241                 i (object).
 242                 strength, angle (float).
 243                 toObstacle, transDir (vector).
 244
 245                 transDir = (self get-rotation) * direction.
 246
 247                 value = 0.0.
 248
 249                 foreach i in (all CeldasObstacles): {
 250                         toObstacle = (i get-location) - (self get-location).
 251                         angle = angle(toObstacle, transDir).
 252
 253                         print "angle: $angle -- sensorAngle = $sensorAngle".
 254                         if angle < sensorAngle: {
 255                                 strength = | (self get-location) - (i get-location) |.
 256                                 strength = 100.0 / (strength * strength) .
 257
 258                                 if strength > value: value = strength.
 259                         }
 260                 }
 261
 262 }