<%= "" %> 17 3 22 0 1.1 <%= Math::PI %> orbit model://sun model://ground_plane 0 2 -9.81 <% # World with several models with varying number of collision spheres # SI units (length in meters) require "matrix" def a_to_s(v) Array(v).join(" ") end # Geometry ball_radius = 0.2 face_altitude = 0.5 side_length = face_altitude / (Math::sqrt(3) / 2.0) ball_volume = 4.0 / 3.0 * Math::PI * ball_radius**3 def sphere_collision(name_suffix, pos, radius, color="Green", slip=1, friction=0.5) """ #{ a_to_s(pos) } 0 0 0 #{ radius } #{ friction } #{ friction } #{ slip } #{ slip } #{ a_to_s(pos) } 0 0 0 #{ radius } """ end models = {} # "lowball" has one collision sphere with a hanging center of mass, # which makes the ball slide instead of roll models["lowball"] = { :inertia_offset => Vector[0, 0, -0.3], :ball_locations => { "a" => Vector[0, 0, 0] , } } # "twoball" has two collision spheres # it will slide in one direction and roll in the other direction models["twoball"] = { :ball_locations => { "a" => Vector[0, side_length / 2.0, 0] , "b" => Vector[0, -side_length / 2.0, 0] , } } # "triball" has three collision spheres arranged in an equilateral triangle # it will slide in any direction models["triball"] = { :ball_locations => { "a" => Vector[ 2.0 / 3.0 * face_altitude, 0, 0] , "b" => Vector[-1.0 / 3.0 * face_altitude, side_length / 2.0, 0] , "c" => Vector[-1.0 / 3.0 * face_altitude, -side_length / 2.0, 0] , } } # there are several pairs of test cases # each pair has the same product of mass and slip, mass_slip = {} mass_slip["0.2a"] = { :mass => 1.0, :slip => 0.2, :color => "Red"} mass_slip["0.2b"] = { :mass => 2.0, :slip => 0.1, :color => "Red"} mass_slip["0.4a"] = { :mass => 2.0, :slip => 0.2, :color => "Green"} mass_slip["0.4b"] = { :mass => 4.0, :slip => 0.1, :color => "Green"} mass_slip["0.6a"] = { :mass => 3.0, :slip => 0.2, :color => "Blue"} mass_slip["0.6b"] = { :mass => 6.0, :slip => 0.1, :color => "Blue"} models.keys.each_with_index do |model_prefix, i| model = models[model_prefix] ball_locations = model[:ball_locations] ball_count = ball_locations.length inertia_offset = model[:inertia_offset] inertia_offset = model.key?(:inertia_offset) ? model[:inertia_offset] : Vector[0, 0, 0] mass_slip.keys.each_with_index do |ms_name, ii| ms = mass_slip[ms_name] mass = ms[:mass] slip = ms[:slip] color = ms[:color] # inertia link_mass = mass ball_mass = link_mass * 1.0/ball_count ball_ixx = 2.0/5.0 * ball_mass * ball_radius**2 ball_iyy = 2.0/5.0 * ball_mass * ball_radius**2 ball_izz = 2.0/5.0 * ball_mass * ball_radius**2 # center of mass (com) and lumped inertia of balls ball_com = Vector[0, 0, 0] ball_locations.keys.each do |k| ball_com += ball_locations[k] / ball_count end link_ixx = ball_count * ball_ixx link_iyy = ball_count * ball_iyy link_izz = ball_count * ball_izz link_ixy = 0.0 link_ixz = 0.0 link_iyz = 0.0 ball_locations.keys.each do |k| dx = ball_locations[k][0] - ball_com[0] dy = ball_locations[k][1] - ball_com[1] dz = ball_locations[k][2] - ball_com[2] link_ixx += ball_mass * (dy**2 + dz**2) link_iyy += ball_mass * (dz**2 + dx**2) link_izz += ball_mass * (dx**2 + dy**2) link_ixy -= ball_mass * dx*dy link_ixz -= ball_mass * dx*dz link_iyz -= ball_mass * dy*dz end model_name = "#{model_prefix}_#{ms_name}" model_pose = Vector[2*ii, -2*i, 0, 0, 0, 0] %> <%= a_to_s(model_pose) %> 0 0 <%= ball_radius %> 0 0 0 <%= a_to_s(ball_com + inertia_offset) %> 0 0 0 <%= link_mass %> <%= link_ixx %> <%= link_iyy %> <%= link_izz %> <%= link_ixy %> <%= link_ixz %> <%= link_iyz %> <% # Place sphere collision and visual at each corner ball_locations.keys.each_index do |first| key = ball_locations.keys[first] pos = ball_locations[key] %> <%= sphere_collision(key, pos, ball_radius, color, slip) %> <% end %> <% end end %>