bulldog_physics 0.1.1 → 0.2.0

data/lib/RigidBodies/force_generator.rb

@@ -0,0 +1,7 @@
+module BulldogPhysics
+	class ForceGenerator
+
+		virtual :update_force
+
+	end
+end

data/lib/RigidBodies/force_registration.rb

@@ -0,0 +1,12 @@
+module BulldogPhysics
+	class ForceRegistration
+
+ 		attr_accessor :body, :force_generator
+  
+      	def initialize()
+        	@body = RigidBody.new
+        	@pforce_generator = ForceGenerator.new
+		end
+		
+	end
+end

data/lib/RigidBodies/force_registry.rb

@@ -0,0 +1,48 @@
+module BulldogPhysics
+  class ForceRegistry
+    
+
+    attr_accessor :registrations
+
+    def initialize()
+      @registrations = Array.new # array of ParticleForceRegistration(s)
+    end
+
+    # Registers the given force generator to apply to the 
+    # given particle.
+    def add(body, force_generator)
+      body_registration = ForceRegistration.new
+      body_registration.body = body
+      body_registration.force_generator = force_generator
+      @registrations << body_registration
+      puts "Registration count #{@registrations.count}"
+    end
+
+
+    # Removes the given registered pair from the registry.
+    # If the pair is not registered, this method will have
+    # no effect.
+    def remove(body,force_generator)
+      @registrations.remove_if do |registry|
+        registry.body.eql? body and registry.force_generator.eql? force_generator
+      end
+    end
+
+    ### Clears all registrations from the registry. This will
+    ### not delete the particles or the force generators
+    ### themselves, just the records of their connection.
+    def clear
+      @registrations = Array.new
+    end
+
+
+    #Calls all the force generators to update the forces of
+    #their corresponding particles.
+    def update_forces(duration)
+      for registry in @registrations
+        registry.force_generator.update_force(registry.body,duration)
+      end
+    end
+
+  end
+end

data/lib/RigidBodies/joint.rb

@@ -0,0 +1,45 @@
+module BulldogPhysics
+	class Joint < ContactGenerator
+
+		attr_accessor :body, :position, :error
+
+
+		def initialize()
+			@body = [RigidBody.new, RigidBody.new]
+			@position = [Vector3.new, Vector3.new]
+			@error = 0.0
+		end
+		def add_contact(contacts, limit)
+			a_pos_world = @body[0].get_point_in_world_space(@position[0])
+			b_pos_world = @body[1].get_point_in_world_space(@position[1])
+			a_to_b = b_pos_world - a_pos_world
+			normal = a_to_b.unit
+
+			length = a_to_b.magnitude
+
+			if(length.abs > error)
+				puts "add contact"
+				contact = Contact.new
+				contact.body = @body
+				contact.contact_normal = normal
+				contact.contact_point = (a_pos_world + b_pos_world) * 0.5
+				contact.penetration = length - error
+				contact.friction = 1.0
+				contact.restitution = 0
+				contacts << contact
+				return 1
+			end
+			return 0
+		end
+
+		def set(a, a_pos, b, b_pos, error)
+			@body[0] = a 
+			@body[1] = b
+
+			@position[0] = a_pos
+			@position[1] = b_pos
+
+			@error = error
+		end
+	end
+end

data/lib/RigidBodies/rigid_body.rb

@@ -0,0 +1,261 @@
+module BulldogPhysics
+	class RigidBody
+
+
+
+		attr_accessor :inverse_mass, :linear_damping, :angular_damping, :position, :velocity, :rotation, :transform_matrix, :inverse_inertia_tensor_world,
+					   :force_accum, :torque_accum, :acceleration, :material, :frozen, :last_frame_acceleration, :motion, :sleep_epsilon
+
+		attr :mass
+
+		attr_reader :mass, :is_awake, :orientation
+		# Holds the inverse of the body’s inertia tensor. The
+		# intertia tensor provided must not be degenerate
+		# (that would mean the body had zero inertia for
+		# spinning along one axis). As long as the tensor is
+		# finite, it will be invertible. The inverse tensor
+		# is used for similar reasons to the use of inverse * mass.
+		#
+		# The inertia tensor, unlike the other variables that
+		# define a rigid body, is given in body space.
+		attr :inverse_inertia_tensor
+
+		#public
+
+
+		def initialize()
+			@inverse_mass = 0
+			@mass = 1
+			@linear_damping , @angular_damping = 0.9, 0.9
+			@acceleration = Vector3.new
+			@position  = Vector3.new
+			@velocity = Vector3.new
+			@orientation = Quaternion.new
+			@transform_matrix = Matrix4.new
+			@rotation = Vector3.new
+			@inverse_inertia_tensor = Matrix3.new
+			@inverse_inertia_tensor_world = Matrix3.new
+			@torque_accum = Vector3.new
+			@force_accum = Vector3.new
+			@material = Material.new
+			@last_frame_acceleration = Vector3.new
+			@motion = 0.0
+			@sleep_epsilon = 0.5
+		end
+
+
+		def orientation=(orientation)
+			@orientation = orientation
+			@orientation.normalize
+		end
+
+		def mass=(mass)
+			@mass = mass
+			@inverse_mass = 1.0 / @mass
+		end
+
+
+		def is_awake=(awake)
+			@is_awake = awake
+			if @is_awake
+				@motion = @sleep_epsilon * 2.0
+			else
+				@velocity.clear()
+				@rotation.clear()
+			end
+		end
+		# Calculates internal data from state data. This should be called * after the body’s state is altered directly (it is called
+		# automatically during integration). If you change the body’s state * and then intend to integrate before querying any data (such as
+		# the transform matrix), then you can omit this step.
+		def calculate_derived_data
+			@orientation.normalize()
+			RigidBody.calculate_transform_matrix(@transform_matrix, @position, @orientation)
+			RigidBody.transform_inertia_tensor(@inverse_inertia_tensor_world,
+				@orientation, @inverse_inertia_tensor, @transform_matrix);
+		end
+
+
+		def set_inertia_tensor(inertia_tensor)
+			@inverse_inertia_tensor.setInverse(inertia_tensor)
+		end
+		# Adds the given force to center of mass of the rigid body
+		# The force is expressed in world coordinates.
+		def add_force(force)
+			@force_accum.addVector(force)
+			self.is_awake = true
+		end
+
+		def hasFiniteMass
+			return @mass < 0.0
+		end
+
+
+		def integrate(duration)
+
+			return if !@is_awake
+			
+			@last_frame_acceleration = @acceleration.dup
+			@last_frame_acceleration.addScaledVector(@force_accum, @inverse_mass)
+
+			angular_acceleration = @inverse_inertia_tensor_world.transform(@torque_accum)
+
+			@velocity.addScaledVector( @last_frame_acceleration, duration)
+			@rotation.addScaledVector(angular_acceleration, duration)
+
+			@velocity *= @linear_damping ** duration
+			@rotation *= @angular_damping ** duration
+
+			#$@rotation *= @angular_damping ** duration
+			#@velocity.multiplyByScalar( @linear_damping ** duration)
+			@position.addScaledVector(@velocity, duration)
+			@orientation.addScaledVector(@rotation, duration)
+
+			calculate_derived_data()
+
+			clear_accumulators()
+
+			@can_sleep ||= true
+=begin
+			if can_sleep
+				current_motion = @velocity.scalarProduct(@velocity) + @rotation.scalarProduct(@rotation) 
+				bias = 0.5 ** duration
+				@motion = (bias * @motion) + ((1-@bias)*current_motion)
+
+				if motion < @sleep_epsilon
+					self.is_awake=false
+				elsif @motion > 10 * @sleep_epsilon
+					@motion = 10 * @sleep_epsilon
+				end
+			end
+=end
+		end
+
+		def clear_accumulators()
+			@force_accum.clear
+			@torque_accum.clear
+		end
+
+		def add_force_at_body_point(force, point)
+			pt = get_point_in_world_space(point)
+			add_force_at_point(force, pt)
+			@is_awake = true
+		end
+
+		def add_force_at_point(force, point)
+			pt = point
+			pt -= @position
+			@force_accum += force
+			@torque_accum += pt % force
+			@is_awake = true
+		end
+		# Internal function to do an intertia tensor transform by a quaternion. 
+		# Note that the implementation of this function was created by an
+		# automated code generator and optimizer.
+		def self.transform_inertia_tensor(iitWorld, q, iitBody, rotmat )
+			 t4 = rotmat.data[0]*iitBody.data[0]+
+			rotmat.data[1]*iitBody.data[3]+
+			rotmat.data[2]*iitBody.data[6];
+			 t9 = rotmat.data[0]*iitBody.data[1]+
+			rotmat.data[1]*iitBody.data[4]+
+			rotmat.data[2]*iitBody.data[7];
+			 t14 = rotmat.data[0]*iitBody.data[2]+
+			rotmat.data[1]*iitBody.data[5]+
+			rotmat.data[2]*iitBody.data[8];
+			 t28 = rotmat.data[4]*iitBody.data[0]+
+			rotmat.data[5]*iitBody.data[3]+
+			rotmat.data[6]*iitBody.data[6];
+			 t33 = rotmat.data[4]*iitBody.data[1]+
+			rotmat.data[5]*iitBody.data[4]+
+			rotmat.data[6]*iitBody.data[7];
+			 t38 = rotmat.data[4]*iitBody.data[2]+
+			rotmat.data[5]*iitBody.data[5]+
+			rotmat.data[6]*iitBody.data[8];
+			 t52 = rotmat.data[8]*iitBody.data[0]+
+			rotmat.data[9]*iitBody.data[3]+
+			rotmat.data[10]*iitBody.data[6];
+			 t57 = rotmat.data[8]*iitBody.data[1]+
+			rotmat.data[9]*iitBody.data[4]+
+			rotmat.data[10]*iitBody.data[7];
+			 t62 = rotmat.data[8]*iitBody.data[2]+
+			rotmat.data[9]*iitBody.data[5]+ rotmat.data[10]*iitBody.data[8];
+			iitWorld.data[0] = t4*rotmat.data[0]+ t9*rotmat.data[1]+ t14*rotmat.data[2];
+			iitWorld.data[1] = t4*rotmat.data[4]+ t9*rotmat.data[5]+ t14*rotmat.data[6];
+			iitWorld.data[2] = t4*rotmat.data[8]+ t9*rotmat.data[9]+ t14*rotmat.data[10];
+			iitWorld.data[3] = t28*rotmat.data[0]+ t33*rotmat.data[1]+
+			t38*rotmat.data[2]; iitWorld.data[4] = t28*rotmat.data[4]+
+			t33*rotmat.data[5]+
+			t38*rotmat.data[6]; iitWorld.data[5] = t28*rotmat.data[8]+
+			t33*rotmat.data[9]+
+			t38*rotmat.data[10]; iitWorld.data[6] = t52*rotmat.data[0]+
+			t57*rotmat.data[1]+
+			t62*rotmat.data[2]; iitWorld.data[7] = t52*rotmat.data[4]+
+			t57*rotmat.data[5]+
+			t62*rotmat.data[6]; iitWorld.data[8] = t52*rotmat.data[8]+
+			t57*rotmat.data[9]+ t62*rotmat.data[10];
+
+		end
+
+		def self.calculate_transform_matrix(transformMatrix, position, orientation)
+			transformMatrix.data[0] = 1-2*orientation.j*orientation.j -
+									2*orientation.k*orientation.k; 
+			transformMatrix.data[1] = 2*orientation.i*orientation.j -
+									2*orientation.r*orientation.k; 
+			transformMatrix.data[2] = 2*orientation.i*orientation.k +
+									 2*orientation.r*orientation.j; 
+			transformMatrix.data[3] = position.x;
+
+			transformMatrix.data[4] = 2*orientation.i*orientation.j + 
+									  2*orientation.r*orientation.k;
+			transformMatrix.data[5] = 1-2*orientation.i*orientation.i - 
+										2*orientation.k*orientation.k;
+			transformMatrix.data[6] = 2*orientation.j*orientation.k - 
+									  2*orientation.r*orientation.i;
+			transformMatrix.data[7] = position.y;
+			transformMatrix.data[8] = 2*orientation.i*orientation.k - 
+									  2*orientation.r*orientation.j;
+			transformMatrix.data[9] = 2*orientation.j*orientation.k + 
+			2*orientation.r*orientation.i;
+			transformMatrix.data[10] = 1-2*orientation.i*orientation.i - 
+										2*orientation.j*orientation.j;
+			transformMatrix.data[11] = position.z;
+		end
+
+
+		def get_point_in_local_space(point)
+    		@transform_matrix.transformInverse(point);
+    	end
+
+		def get_point_in_world_space(point)
+    		@transform_matrix.transform(point);
+    	end
+
+    	def get_gl_transform()
+    		matrix = Array.new(16)
+		    matrix[0] = @transform_matrix.data[0];
+		    matrix[1] = @transform_matrix.data[4];
+		    matrix[2] = @transform_matrix.data[8];
+		    matrix[3] = 0;
+
+		    matrix[4] = @transform_matrix.data[1];
+		    matrix[5] = @transform_matrix.data[5];
+		    matrix[6] = @transform_matrix.data[9];
+		    matrix[7] = 0;
+
+		    matrix[8] = @transform_matrix.data[2];
+		    matrix[9] = @transform_matrix.data[6];
+		    matrix[10] = @transform_matrix.data[10];
+		    matrix[11] = 0;
+
+		    matrix[12] = @transform_matrix.data[3];
+		    matrix[13] = @transform_matrix.data[7];
+		    matrix[14] = @transform_matrix.data[11];
+		    matrix[15] = 1;
+
+		    matrix
+    	end
+		# def method_missing(meth, *args, &blk)
+		# 	puts "[RigidBody] method #{meth} is missing"
+		# end
+	end
+
+end

data/lib/RigidBodies/rigid_collisions.rb

@@ -0,0 +1,292 @@
+module BulldogPhysics
+	class BoundingSphere
+		attr_accessor :center, :radius
+
+		def initialize(*args)
+			if args.size > 1
+				if args[0].is_a? Vector3
+					@center = args[0]
+					@radius = args[1]
+				elsif args[0].is_a?  BoundingSphere
+					initialize_with_bounding_spheres(args[0],args[1])
+				end
+			end
+		end
+
+		def initialize_with_bounding_spheres(one, two)
+			center_offset = two.center - one.center
+			distance = center_offset.squareMagnitude()
+			radius_diff = two.radius - one.radius
+
+			if radius_diff * radius_diff >= distance
+				if one.radius > two.radius
+					@center = one.center
+					@radius = one.radius
+				else
+					@center = two.center
+					@radius = two.radius
+				end
+			else
+				distance = Math.sqrt(distance)
+				@radius = (distance + one.radius + two.radius) * 0.5
+				@center = one.center
+
+				if distance > 0
+					@center += center_offset * ((@radius - one.radius) / distance)
+				end
+			end
+
+		end
+
+		def overlaps(other)
+			distance_squared = (@center - other.center).squareMagnitude
+			return distance_squared < (@radius + other.radius)*(@radius + other.radius)
+		end
+
+
+	end
+
+	class BoundingBox
+		attr :center, :half_of_size
+
+		def initialize(center = Vector3.new, half_of_size = Vector3.new)
+			@center = center
+			@half_of_size = half_of_size
+		end
+	end
+
+	class Plane
+		attr_accessor :position, :direction
+
+		def initialize()
+		end
+	end
+
+	# BSPChildType enum
+	module BSPChildType
+		NODE = 0
+		OBJECTS = 1
+	end
+
+
+=begin
+	class BSPElement
+
+		attr_accessor :type # BSPChildType
+		attr_accessor :BSPNode
+
+	end
+=end
+	class BSPElement
+	end
+
+	
+	class BSPObjectSet < BSPElement
+		attr_accessor :objects
+
+		def initialize(objects = Array.new)
+			@objects = objects
+		end
+	end
+
+	class BSPNode < BSPElement
+		attr_accessor :plane, :front, :back
+
+		def initialize(plane = Plane.new, front = BSPElement.new, back = BSPElement.new)
+			@plane, @front, @back = plane, front, back
+		end
+
+	end
+
+
+	class QuadTreeSector
+		BOTTOM_LEFT = 0
+		BOTTOM_RIGHT = 1
+		TOP_LEFT = 2
+		TOP_RIGHT = 3
+	end
+
+	class OctTreeSector
+		LOWER_BOTTOM_LEFT = 0
+		LOWER_BOTTOM_RIGHT = 1
+		HIGHER_BOTTOM_LEFT = 2
+		HIGHER_BOTTOM_RIGHT = 3
+		LOWER_TOP_LEFT = 4
+		LOWER_TOP_RIGHT = 5
+		HIGHER_TOP_LEFT = 6
+		HIGHER_TOP_RIGHT = 7
+	end
+
+	class QuadTreeNode
+		attr_accessor :position, :children
+
+		def initialize()
+			@position = Vector3.new
+			@children = Array.new(4, QuadTreeNode.new)
+		end
+
+		def get_child_index(object_centre)
+			index = 0
+			index += 1 if object_centre.x > @position.x
+			index += 2 if object_centre.z > @position.z
+			index
+		end
+	end
+
+	class OctTreeNode
+		attr_accessor :position, :children
+
+		def initialize()
+			@position = Vector3.new
+			@children = Array.new(8, OctTreeNode.new)
+		end
+
+		def get_child_index(object_centre)
+			index = 0
+			index += 1 if object_centre.x > @position.x
+			index += 2 if object_centre.y > @position.y
+			index += 4 if object_centre.z > @position.z
+			index
+		end
+	end
+
+	class Grid
+		attr_accessor :x_extent, :z_extent, :locations, :origin, :inverse_cell_size, :active_sets
+
+		def initialize
+		end
+
+		def get_location_index(object_centre)
+			square = object_centre.componentProduct(inverseCellSize)
+			square.x + @x_extent * square.z
+		end
+
+		def add(object)
+			location = get_location_index(object.center)
+			set = @locations[location]
+			@active_sets.insert(set) if(set.size() == 1)
+			set.add(object)
+		end
+
+		def remove(object)
+			location = get_location_index(object.center)
+			set = @locations[location]
+			@active_sets.erase(set) if set.size == 2
+			set.remove(object)
+		end
+	end
+
+	class PotentialContact
+		attr :bodies
+
+		def initialize(bodies = Array.new)
+			@bodies = bodies
+		end
+	end
+
+	class BVHNode
+
+		attr_accessor :children, :volume, :body, :parent
+
+		def initialize()
+			@children = Array.new
+			@volume = BHVNode.new
+		end
+
+		def is_leaf
+			!@body.nil?
+		end
+
+		def get_potential_contacts(contacts, limit)
+			return if limit == 0 or is_leaf
+
+			@children[0].get_potential_contacts_with(@children[1], contacts, limit)
+		end
+
+		def get_potential_contacts_with(other, contacts,limit)
+			return if !overlaps(other) or limit == 0
+
+			if is_leaf or other.is_leaf
+				contacts.body[0] = @body
+				contacts.body[1] = other.body
+			end
+
+			if other.is_leaf or (!is_leaf and @volume.get_size >= other.volume.get_size)
+
+				count = @children[0].get_potential_contacts_with(other,contacts,limit)
+
+				if limit > count
+					return count + @children.get_potential_contacts_with(other, contacts[count], limit - count)
+				else
+					return count
+				end
+
+			else
+				count = get_potential_contacts_with(other.children[0], contacts, limit)
+
+				if limit > count
+					return count + get_potential_contacts_with(other.children[1], contacts[count], limit - count)
+				else
+					return count
+				end
+
+			end
+		end
+
+
+		def insert(new_body, new_volume)
+			
+			if is_leaf
+				@children[0] = BHVNode.new(self, @volume, @body)
+				@children[1] = BHVNode.new(self, new_volume, new_body)
+
+				self.body = nil
+
+				recalculate_bounding_volume()
+			else
+				if @children[0].volume.get_growth(new_volume) < @children[1].get_growth(new_volume)
+					@children[0].insert(new_body, new_volume)
+				else
+					@children[1].insert(new_body, new_volume)
+				end
+			end
+		end
+
+		def overlaps(other)
+			@volume.overlaps(other.volume)
+		end
+
+		def destroy_node
+			if !@parent.nil?
+				if @parent.children[0].eql? self
+					sibling = @parent.children[1]
+				else
+					sibling = @parent.children[0]
+				end
+
+				@parent.volume = sibling.volume
+				@parent.body = sibling.volume
+				@parent.children[0] = sibling.children[0]
+				@parent.children[1] = sibling.children[1]
+
+
+				sibling.parent = nil
+				sibling.body = nil
+				sibling.children = nil
+
+				@parent.recalculate_bounding_volume()
+			end
+
+			if( @children[0])
+				@children[0].parent = nil
+			end
+
+			if( @children[1])
+				@children[1].parent = nil
+			end
+
+		end
+
+	end
+
+end