Class: BulldogPhysics::Contact

Inherits:
Object
  • Object
show all
Defined in:
lib/RigidBodies/contact.rb

Instance Attribute Summary collapse

Instance Method Summary collapse

Constructor Details

#initialize ⇒ Contact

Returns a new instance of Contact.



47
48
49
50
51
52
53
54
55
56
57
 
# File 'lib/RigidBodies/contact.rb', line 47

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

Instance Attribute Details

#body ⇒ Object

Holds the bodies that are involved in the contact. The second of these can be NULL, for contacts with the scenery.



7
8
9
 
# File 'lib/RigidBodies/contact.rb', line 7

def body
  @body
end

#contact_normal ⇒ Object

Holds the direction of the contact in world coordinates.



19
20
21
 
# File 'lib/RigidBodies/contact.rb', line 19

def contact_normal
  @contact_normal
end

#contact_point ⇒ Object

Holds the position of the contact in world coordinates.



16
17
18
 
# File 'lib/RigidBodies/contact.rb', line 16

def contact_point
  @contact_point
end

#contact_to_world ⇒ Object

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.



30
31
32
 
# File 'lib/RigidBodies/contact.rb', line 30

def contact_to_world
  @contact_to_world
end

#contact_velocity ⇒ Object

Holds the closing velocity at the point of contact. This is set when the calculateInternals function is run.



34
35
36
 
# File 'lib/RigidBodies/contact.rb', line 34

def contact_velocity
  @contact_velocity
end

#desired_delta_velocity ⇒ Object

Holds the required change in velocity for this contact to be resolved.



38
39
40
 
# File 'lib/RigidBodies/contact.rb', line 38

def desired_delta_velocity
  @desired_delta_velocity
end

#friction ⇒ Object

Holds the lateral friction coefficient at the contact.



10
11
12
 
# File 'lib/RigidBodies/contact.rb', line 10

def friction
  @friction
end

#penetration ⇒ Object

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.



24
25
26
 
# File 'lib/RigidBodies/contact.rb', line 24

def penetration
  @penetration
end

#relative_contact_position ⇒ Object

Holds the world space position of the contact point relative to centre of each body. This is set when the calculateInternals function is run.



43
44
45
 
# File 'lib/RigidBodies/contact.rb', line 43

def relative_contact_position
  @relative_contact_position
end

#restitution ⇒ Object

Holds the normal restitution coefficient at the contact.



13
14
15
 
# File 'lib/RigidBodies/contact.rb', line 13

def restitution
  @restitution
end

Instance Method Details

#apply_impulse(impulse, body, velocity_change, rotation_change) ⇒ Object 



190
191
192
 
# File 'lib/RigidBodies/contact.rb', line 190

def apply_impulse(impulse, body, velocity_change, rotation_change)
    puts "not done yet"
end

#apply_position_change(linear_change, angular_change, penetration) ⇒ Object 



232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
 
# File 'lib/RigidBodies/contact.rb', line 232

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

#apply_velocity_change(velocity_change, rotation_change) ⇒ Object 



194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
 
# File 'lib/RigidBodies/contact.rb', line 194

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

#calculate_contact_basis ⇒ Object 



165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
 
# File 'lib/RigidBodies/contact.rb', line 165

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

#calculate_desired_delta_velocity(duration) ⇒ Object 



118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
 
# File 'lib/RigidBodies/contact.rb', line 118

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

#calculate_friction_impulse(inverse_inertia_tensor) ⇒ Object 



303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
 
# File 'lib/RigidBodies/contact.rb', line 303

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

#calculate_frictionless_impulse(inverse_inertia_tensor) ⇒ Object 



358
359
360
 
# File 'lib/RigidBodies/contact.rb', line 358

def calculate_frictionless_impulse(inverse_inertia_tensor)
    puts "not implemented"
end

#calculate_internals(duration) ⇒ Object

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.



72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
 
# File 'lib/RigidBodies/contact.rb', line 72

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

#calculate_local_velocity(body_index, duration) ⇒ Object 



145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
 
# File 'lib/RigidBodies/contact.rb', line 145

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

#match_awake_state ⇒ Object 



102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
 
# File 'lib/RigidBodies/contact.rb', line 102

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

#set_body_data(one, two, friction, restitution) ⇒ Object 



59
60
61
62
63
64
 
# File 'lib/RigidBodies/contact.rb', line 59

def set_body_data(one, two, friction, restitution)
    @body[0] = one
    @body[1] = two
    @friction = friction
    @restitution = restitution
end

#swap_bodies ⇒ Object

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).



95
96
97
98
99
100
 
# File 'lib/RigidBodies/contact.rb', line 95

def swap_bodies
    @contact_normal *= -1
    temp = @body[0]
    @body[0] = @body[1]
    @body[1] = temp
end