bulldog_physics 0.1.1 → 0.2.0

data/lib/RigidBodies/contact.rb

@@ -0,0 +1,362 @@
+module BulldogPhysics
+   class Contact
+
+
+        # Holds the bodies that are involved in the contact. The
+        # second of these can be NULL, for contacts with the scenery.
+        attr_accessor :body
+
+        # Holds the lateral friction coefficient at the contact.
+        attr_accessor :friction
+        
+        # Holds the normal restitution coefficient at the contact.
+        attr_accessor :restitution
+
+        # Holds the position of the contact in world coordinates.
+        attr_accessor :contact_point
+
+         # Holds the direction of the contact in world coordinates.
+        attr_accessor :contact_normal
+
+        # Holds the depth of penetration at the contact point. If both
+        # bodies are specified then the contact point should be midway
+        # between the inter-penetrating points.
+        attr_accessor :penetration
+
+
+        # A transform matrix that converts co-ordinates in the contact's
+        # frame of reference to world co-ordinates. The columns of this
+        # matrix form an orthonormal set of vectors.
+        attr_accessor :contact_to_world
+
+        # Holds the closing velocity at the point of contact. This is set
+        # when the calculateInternals function is run.
+        attr_accessor :contact_velocity
+
+        # Holds the required change in velocity for this contact to be
+        # resolved.
+        attr_accessor :desired_delta_velocity
+
+        # Holds the world space position of the contact point relative to
+        # centre of each body. This is set when the calculateInternals
+        # function is run.
+        attr_accessor :relative_contact_position
+
+
+
+        def initialize()
+            @body = Array.new(2)
+            @contact_normal = Vector3.new
+            @contact_point = Vector3.new
+            @contact_to_world = Matrix3.new
+            @contact_velocity = Vector3.new
+            @desired_delta_velocity = Vector3.new
+            @relative_contact_position = [ Vector3.new, Vector3.new ]
+            @penetration = 0.0
+            set_body_data(RigidBody.new, RigidBody.new, 1.0, 0.96)
+        end
+
+        def set_body_data(one, two, friction, restitution)
+            @body[0] = one
+            @body[1] = two
+            @friction = friction
+            @restitution = restitution
+        end
+
+
+
+
+        # Calculates internal data from state data. This is called before
+        # the resolution algorithm tries to do any resolution. It should
+        # never need to be called manually.
+        def calculate_internals(duration)
+            swap_bodies if @body[0].nil?
+
+            calculate_contact_basis
+
+            @relative_contact_position[0] = @contact_point - @body[0].position
+
+            unless @body[1].nil?
+                @relative_contact_position[1] = @contact_point - @body[1].position
+            end
+
+            @contact_velocity = calculate_local_velocity(0, duration)
+            unless @body[1].nil?
+                @contact_velocity -= calculate_local_velocity(1, duration)
+            end
+
+            calculate_desired_delta_velocity(duration)
+        end
+
+        # Reverses the contact. This involves swapping the two rigid bodies
+        # and reversing the contact normal. The internal values should then
+        # be recalculated using calculateInternals (this is not done
+        # automatically).
+        def swap_bodies
+            @contact_normal *= -1
+            temp = @body[0]
+            @body[0] = @body[1]
+            @body[1] = temp
+        end
+
+        def match_awake_state
+
+            return if @body[1].nil?
+
+            body0awake = @body[0].is_awake
+            body1awake = @body[1].is_awake
+
+            if(body0awake != body1awake)
+                if(body0awake)
+                    @body[1].is_awake = true
+                else
+                    @body[0].is_awake = true
+                end
+            end
+        end
+
+        def calculate_desired_delta_velocity(duration)
+
+            velocity_limit = 0.25
+
+            velocity_from_acc = 0.0
+
+
+            if @body[0].is_awake
+                velocity_from_acc = @body[0].last_frame_acceleration.scalarProduct(@contact_normal) * duration
+                #@body[0].last_frame_acceleration.componentProductUpdate(@contact_normal * duration)
+            end
+
+            if !@body[1].nil?
+                if @body[1].is_awake
+                    #@body[1].last_frame_acceleration.componentProductUpdate(@contact_normal * duration)
+                    velocity_from_acc -= @body[1].last_frame_acceleration.scalarProduct(@contact_normal) * duration
+                end
+            end
+
+            this_restitution = @restitution
+            if(@contact_velocity.x.abs < velocity_limit)
+                this_restitution = 0.0
+            end
+
+            @desired_delta_velocity = -@contact_velocity.x - (this_restitution * (@contact_velocity.x - velocity_from_acc))
+        end
+
+        def calculate_local_velocity(body_index, duration)
+            this_body = @body[body_index]
+
+            velocity = this_body.rotation % @relative_contact_position[body_index]
+            velocity += this_body.velocity
+
+            contact_velocity = @contact_to_world.transform_transpose(velocity)
+
+            #puts this_body.last_frame_acceleration
+            acc_velocity = this_body.last_frame_acceleration * duration
+            #puts acc_velocity
+            acc_velocity = @contact_to_world.transform_transpose(acc_velocity)
+
+            acc_velocity.x = 0
+
+            contact_velocity += acc_velocity
+
+            contact_velocity
+        end
+
+        def calculate_contact_basis
+            contact_tangent = [Vector3.new, Vector3.new]
+            if(@contact_normal.x.abs > @contact_normal.y.abs)
+                s = 1.0 / Math.sqrt( @contact_normal.z * @contact_normal.z + @contact_normal.x * @contact_normal.x)
+                contact_tangent[0].x = @contact_normal.z * s
+                contact_tangent[0].y = 0
+                contact_tangent[0].z = -@contact_normal.x * s
+
+                contact_tangent[1].x = @contact_normal.y * contact_tangent[0].x
+                contact_tangent[1].y = @contact_normal.z * contact_tangent[0].x - @contact_normal.x * contact_tangent[0].z
+                contact_tangent[1].z = -@contact_normal.y * contact_tangent[0].x
+            else
+                s = 1.0 / Math.sqrt( @contact_normal.z * @contact_normal.z + @contact_normal.y * @contact_normal.y)
+                contact_tangent[0].x = 0
+                contact_tangent[0].y = -@contact_normal.z * s
+                contact_tangent[0].z = @contact_normal.y * s
+
+                contact_tangent[1].x = @contact_normal.y * contact_tangent[0].z - @contact_normal.z * contact_tangent[0].y
+                contact_tangent[1].y = -@contact_normal.x * contact_tangent[0].z
+                contact_tangent[1].z = @contact_normal.x * contact_tangent[0].y 
+            end
+
+            @contact_to_world.setComponents( @contact_normal, contact_tangent[0], contact_tangent[1])
+        end
+
+        def apply_impulse(impulse, body, velocity_change, rotation_change)
+            puts "not done yet"
+        end
+
+        def apply_velocity_change(velocity_change, rotation_change)
+            inverse_inertia_tensors = [Matrix3.new, Matrix3.new ]
+
+            inverse_inertia_tensors[0] = @body[0].inverse_inertia_tensor_world.dup
+
+            unless @body[1].nil?
+                inverse_inertia_tensors[1] = @body[1].inverse_inertia_tensor_world.dup
+            end
+
+            impulse_contact = Vector3.new
+
+            if( @friction == 0.0)
+                impulse_contact = calculate_frictionless_impulse(inverse_inertia_tensors)
+            else
+                impulse_contact = calculate_friction_impulse(inverse_inertia_tensors)
+            end
+
+            impulse = @contact_to_world.transform(impulse_contact)
+
+            impulsive_torque = @relative_contact_position[0] % impulse
+            rotation_change[0] = inverse_inertia_tensors[0].transform(impulsive_torque)
+            velocity_change[0].clear()
+            velocity_change[0].addScaledVector(impulse, @body[0].inverse_mass)
+
+            @body[0].velocity += velocity_change[0]
+            @body[0].rotation += rotation_change[0]
+
+            unless @body[1].nil?
+                impulsive_torque = impulse % @relative_contact_position[1]
+                rotation_change[1] = inverse_inertia_tensors[1].transform(impulsive_torque)
+                velocity_change[1].clear()
+                velocity_change[1].addScaledVector(impulse, -@body[1].inverse_mass)
+
+                @body[1].velocity += velocity_change[1]
+                @body[1].rotation += rotation_change[1]
+            end
+        end
+
+        def apply_position_change(linear_change, angular_change, penetration)
+
+            angularLimit = 0.2;
+            angularMove = [0.0, 0.0]
+            linearMove = [0.0, 0.0];
+
+            totalInertia = 0.0;
+            linearInertia = [0.0, 0.0]
+            angularInertia = [0.0, 0.0]
+
+            for i in 0..1
+
+                break if @body[i].nil?
+                
+                inverseInertiaTensor = @body[i].inverse_inertia_tensor_world.dup
+                angularInertiaWorld = @relative_contact_position[i] % @contact_normal
+                angularInertiaWorld = inverseInertiaTensor.transform(angularInertiaWorld)
+                angularInertiaWorld = angularInertiaWorld % @relative_contact_position[i]
+                angularInertia[i] = angularInertiaWorld * @contact_normal
+
+                linearInertia[i] = body[i].inverse_mass
+                totalInertia += linearInertia[i] + angularInertia[i]
+
+            end
+
+            for i in 0..1
+
+                return if @body[i].nil? 
+
+                sign = i == 0 ? 1 : -1
+
+                angularMove[i] = sign * penetration * (angularInertia[i] / totalInertia)
+                linearMove[i] = sign * penetration * (linearInertia[i] / totalInertia)
+
+                projection = @relative_contact_position[i]
+                projection.addScaledVector( @contact_normal, -@relative_contact_position[i].scalarProduct(@contact_normal))
+
+                maxMagnitude = angularLimit * projection.magnitude
+
+                if(angularMove[i] < -maxMagnitude)
+                    totalMove = angularMove[i] + linearMove[i]
+                    angularMove[i] = -maxMagnitude
+                    linearMove[i] = totalMove - angularMove[i]
+                elsif angularMove[i] > maxMagnitude
+                    totalMove = angularMove[i] + linearMove[i]
+                    angularMove[i] = maxMagnitude
+                    linearMove[i] = totalMove - angularMove[i]
+                end
+
+                if(angularMove[i] == 0)
+                    angular_change[i].clear
+                else
+                    targetAngularDirection = @relative_contact_position[i].vector_product(@contact_normal)
+                    inverse_inertia_tensor = @body[i].inverse_inertia_tensor_world.dup
+
+                    angular_change[i] = inverse_inertia_tensor.transform(targetAngularDirection) * (angularMove[i] / angularInertia[i])
+                end
+
+
+                linear_change[i] = @contact_normal * linearMove[i]
+                @body[i].position.addScaledVector( @contact_normal, linearMove[i])
+
+                #puts "WORK"
+                @body[i].orientation.addScaledVector(angular_change[i], 1.0)
+                @body[i].orientation.normalize
+
+                @body[i].calculate_derived_data if !@body[i].is_awake
+            end
+        end
+
+
+        def calculate_friction_impulse(inverse_inertia_tensor)
+
+            impulse_contact = Vector3.new
+            inverse_mass = @body[0].inverse_mass
+
+            impulseToTorque = Matrix3.new
+            impulseToTorque.setSkewSymmetric(@relative_contact_position[0])
+
+            deltaVelWorld = impulseToTorque.dup
+            deltaVelWorld *= inverse_inertia_tensor[0]
+            deltaVelWorld *= impulseToTorque
+            deltaVelWorld *= -1.0
+
+            unless @body[1].nil?
+                impulseToTorque.setSkewSymmetric(@relative_contact_position[1])
+                deltaVelWorld2 = impulseToTorque.dup
+                deltaVelWorld2 *= inverse_inertia_tensor[1]
+                deltaVelWorld2 *= impulseToTorque
+                deltaVelWorld2 *= -1.0
+
+                deltaVelWorld += deltaVelWorld2
+
+                inverse_mass += @body[1].inverse_mass
+            end
+
+            deltaVelocity = @contact_to_world.transpose()
+            deltaVelocity *= deltaVelWorld
+            deltaVelocity *= @contact_to_world
+
+            deltaVelocity.data[0] += inverse_mass
+            deltaVelocity.data[4] += inverse_mass
+            deltaVelocity.data[4] += inverse_mass
+
+            impulse_matrix = deltaVelocity.inverse
+
+            velKill = Vector3.new( @desired_delta_velocity, -@contact_velocity.y, -@contact_velocity.z)
+
+            impulse_contact = impulse_matrix.transform(velKill)
+
+            planarImpulse = Math.sqrt( (impulse_contact.y * impulse_contact.y) + (impulse_contact.z * impulse_contact.z))
+
+            if planarImpulse > impulse_contact.x * @friction
+                impulse_contact.y /= planarImpulse
+                impulse_contact.z /= planarImpulse
+
+                impulse_contact.x = deltaVelocity.data[0] + ( deltaVelocity.data[1] * @friction * impulse_contact.y) + (deltaVelocity.data[2] * @friction * impulse_contact.z)
+                impulse_contact.x = @desired_delta_velocity / impulse_contact.z
+                impulse_contact.y *= @friction * impulse_contact.x;
+                impulse_contact.z *= @friction * impulse_contact.x
+            end
+
+            return impulse_contact
+
+        end        
+
+        def calculate_frictionless_impulse(inverse_inertia_tensor)
+            puts "not implemented"
+        end
+    end
+end

data/lib/RigidBodies/contact_generator.rb

@@ -0,0 +1,7 @@
+module BulldogPhysics
+	class ContactGenerator
+
+		#virtual unsigned addContact(Contact *contact, unsigned limit) const = 0;
+		virtual :add_contact
+	end
+end

data/lib/RigidBodies/contact_resolver.rb

@@ -0,0 +1,143 @@
+module BulldogPhysics
+	class ContactResolver
+
+		attr :velocity_iterations, :position_iterations
+
+		attr :velocity_epsilon, :position_epsilon
+
+		attr_accessor :velocity_iterations_used, :position_iterations_used
+
+		attr :valid_settings
+
+
+		def initialize(iterations, velocity_epsilon = 0.01, position_epsilon = 0.01)
+			set_iterations(iterations, iterations)
+			set_epsilon(velocity_epsilon, position_epsilon)
+		end
+
+		def is_valid
+			@velocity_iterations > 0 && @position_iterations > 0 && @position_epsilon >= 0.0 && @velocity_epsilon >= 0
+		end
+
+		def set_iterations(velocity_iterations, position_iterations)
+			@velocity_iterations = velocity_iterations
+			@position_iterations = position_iterations
+		end
+
+		def set_epsilon(velocity_epsilon, position_epsilon)
+			@velocity_epsilon = velocity_epsilon
+			@position_epsilon = position_epsilon
+		end
+
+		def resolve_contacts(contact_array, num_contacts, duration)
+			return if num_contacts == 0
+			return unless is_valid
+
+			prepare_contacts(contact_array, num_contacts, duration)
+			adjust_positions(contact_array, num_contacts, duration)
+			adjust_velocities(contact_array, num_contacts, duration)
+		end
+
+		def prepare_contacts(contact_array, num_contacts, duration)
+			contact_array.each do |contact|
+				contact.calculate_internals(duration)
+			end
+		end
+
+		def adjust_velocities(contact_array, num_contacts, duration)
+			velocityChange = [Vector3.new, Vector3.new]
+			rotationChange = [Vector3.new, Vector3.new]
+			deltaVel = Vector3.new
+
+			@velocity_iterations_used = 0
+
+			while @velocity_iterations_used < @velocity_iterations do
+
+				max = @velocity_epsilon
+				index = num_contacts
+				for i in 0..num_contacts-1
+					if(contact_array[i].desired_delta_velocity > max)
+						max = contact_array[i].desired_delta_velocity
+						index = i
+					end
+				end
+
+				break if index == num_contacts
+
+				contact_array[index].match_awake_state
+				contact_array[index].apply_velocity_change(velocityChange, rotationChange)
+
+				for i in i..num_contacts-1
+					for b in 0..1
+						next if contact_array[i].body[b].nil?
+						for d in 0..1
+							if(contact_array[i].body[b] == contact_array[index].body[d])
+								deltaVel = velocityChange[d] + rotationChange[d].vector_product(contact_array[i].relative_contact_position[b]);
+								#puts "DELAT VEL #{deltaVel}"
+								#puts "CONTACT VELOCITY #{contact_array[i].contact_velocity}"
+								#puts "CONTACT TO WORLD "
+
+								change = contact_array[i].contact_to_world.transformTranspose(deltaVel) 
+								if(b)
+									contact_array[i].contact_velocity -= change
+								else
+									contact_array[i].contact_velocity += change
+								end
+
+								contact_array[i].calculate_desired_delta_velocity(duration)
+							end
+						end
+					end
+				end
+				@velocity_iterations_used += 1
+			end
+
+		end
+
+		#c is a contact array
+		def adjust_positions(c, num_contacts, duration)
+			linearChange = [Vector3.new, Vector3.new]
+			angularChange = [Vector3.new, Vector3.new]
+			#angularChange = Vector3.new
+			deltaPosition = Vector3.new
+
+			@position_iterations_used = 0
+			while @position_iterations_used < @position_iterations
+
+				max = @position_epsilon
+				index = num_contacts
+
+				for i in 0..num_contacts
+					if( c[i].penetration > max)
+						max = c[i].penetration
+						puts "MAX #{max}"
+						index = i
+					end
+				end
+
+				break if (index == num_contacts)
+
+				c[index].match_awake_state
+				c[index].apply_position_change( linearChange, angularChange, max)
+
+				for i in 0..num_contacts-1
+					for b in 0..1
+						#next if c[i].body[b].nil?
+
+						for d in 0..1
+
+							if(c[i].body[b] == c[index].body[d])
+								deltaPosition = linearChange[d] + angularChange[d].vector_product( c[i].relative_contact_position[b])
+								test = b == 1 ? 1 : -1 
+								#puts test
+								c[i].penetration += deltaPosition.scalarProduct(c[i].contact_normal) * test
+							end
+						end
+					end
+				end
+				@position_iterations_used += 1
+			end
+		end
+
+	end
+end