← Back to branch summary

~ubuntu-branches/ubuntu/gutsy/blender/gutsy-security

« back to all changes in this revision

Viewing changes to source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp

  • Committer: Bazaar Package Importer
  • Author(s): Florian Ernst
  • Date: 2005-11-06 12:40:03 UTC
  • mfrom: (1.1.2 upstream)
  • Revision ID: james.westby@ubuntu.com-20051106124003-3pgs7tcg5rox96xg
Tags: 2.37a-1.1
http://bugs.debian.org/333958
* Non-maintainer upload.
* Split out parts of 01_SConstruct_debian.dpatch again: root_build_dir
  really needs to get adjusted before the clean target runs - closes: #333958,
  see #288882 for reference

Show diffs side-by-side

added added

removed removed

Lines of Context:
source/gameengine/Physics/Sumo/Fuzzics/src/SM_Object.cpp
1
1
/**
2
 
 * $Id: SM_Object.cpp,v 1.11 2004/05/13 13:07:38 kester Exp $
 
2
 * $Id: SM_Object.cpp,v 1.20 2005/03/25 10:33:39 kester Exp $
3
3
 * Copyright (C) 2001 NaN Technologies B.V.
4
4
 * The basic physics object.
5
5
 *
42
42
#pragma warning( disable : 4786 )     
43
43
#endif
44
44
 
 
45
#include "MT_assert.h"
 
46
 
45
47
#include "SM_Object.h"
46
48
#include "SM_Scene.h"
47
49
#include "SM_FhObject.h"
49
51
 
50
52
#include "MT_MinMax.h"
51
53
 
52
 
 
53
 
// Tweak parameters
54
 
static const MT_Scalar ImpulseThreshold = -10.;
55
 
static const MT_Scalar FixThreshold = 0.01;
56
 
static const MT_Scalar FixVelocity = 0.01;
 
54
MT_Scalar SM_Object::ImpulseThreshold = -1.0;
 
55
 
 
56
struct Contact
 
57
{
 
58
        SM_Object *obj1;
 
59
        SM_Object *obj2;
 
60
        MT_Vector3 normal;
 
61
        MT_Point3 pos;
 
62
        
 
63
        // Sort objects by height
 
64
        bool operator()(const Contact *a, const Contact *b)
 
65
        {
 
66
                return a->pos[2] < b->pos[2];
 
67
        }
 
68
        
 
69
        Contact(SM_Object *o1, SM_Object *o2, const MT_Vector3 nor, const MT_Point3 p)
 
70
                : obj1(o1),
 
71
                  obj2(o2),
 
72
                  normal(nor),
 
73
                  pos(p)
 
74
        {
 
75
        }
 
76
        
 
77
        Contact()
 
78
        {
 
79
        }
 
80
        
 
81
        void resolve()
 
82
        {
 
83
                if (obj1->m_static || obj2->m_static)
 
84
                {
 
85
                        if (obj1->isDynamic())
 
86
                        {
 
87
                                if (obj1->m_static && obj2->m_static)
 
88
                                {
 
89
                                        if (obj1->m_static < obj2->m_static)
 
90
                                        {
 
91
                                                obj2->m_error -= normal;
 
92
                                                obj2->m_static = obj1->m_static + 1;
 
93
                                        }
 
94
                                        else
 
95
                                        {
 
96
                                                obj1->m_error += normal;
 
97
                                                obj1->m_static = obj2->m_static + 1;
 
98
                                        }
 
99
                                }
 
100
                                else
 
101
                                {
 
102
                                        if (obj1->m_static)
 
103
                                        {
 
104
                                                obj2->m_error -= normal;
 
105
                                                obj2->m_static = obj1->m_static + 1;
 
106
                                        }
 
107
                                        else
 
108
                                        {
 
109
                                                obj1->m_error += normal;
 
110
                                                obj1->m_static = obj2->m_static + 1;
 
111
                                        }
 
112
                                }
 
113
                        }
 
114
                        else
 
115
                        {
 
116
                                obj2->m_error -= normal;
 
117
                                obj2->m_static = 1;
 
118
                        }
 
119
                }
 
120
                else
 
121
                {
 
122
                        // This distinction between dynamic and non-dynamic objects should not be 
 
123
                        // necessary. Non-dynamic objects are assumed to have infinite mass.
 
124
                        if (obj1->isDynamic()) {
 
125
                                MT_Vector3 error = normal * 0.5f;
 
126
                                obj1->m_error += error;
 
127
                                obj2->m_error -= error;
 
128
                        }
 
129
                        else {
 
130
                                // Same again but now obj1 is non-dynamic
 
131
                                obj2->m_error -= normal;
 
132
                                obj2->m_static = obj1->m_static + 1;
 
133
                        }
 
134
                }
 
135
        
 
136
        }
 
137
                
 
138
        
 
139
        typedef std::set<Contact*, Contact> Set;
 
140
};
 
141
 
 
142
static Contact::Set contacts;
 
143
 
57
144
SM_Object::SM_Object(
58
145
        DT_ShapeHandle shape, 
59
146
        const SM_MaterialProps *materialProps,
62
149
        
63
150
        m_dynamicParent(dynamicParent),
64
151
        m_client_object(0),
65
 
        
 
152
        m_physicsClientObject(0),
66
153
        m_shape(shape),
67
154
        m_materialProps(materialProps),
68
155
        m_materialPropsBackup(0),
69
156
        m_shapeProps(shapeProps),
70
157
        m_shapePropsBackup(0),
71
 
        m_object(DT_CreateObject(this, shape)),
72
158
        m_margin(0.0),
73
159
        m_scaling(1.0, 1.0, 1.0),
74
160
        m_reaction_impulse(0.0, 0.0, 0.0),
75
161
        m_reaction_force(0.0, 0.0, 0.0),
76
 
        m_kinematic(false),
77
 
        m_prev_kinematic(false),
78
 
        m_is_rigid_body(false),
79
162
        m_lin_mom(0.0, 0.0, 0.0),
80
163
        m_ang_mom(0.0, 0.0, 0.0),
81
164
        m_force(0.0, 0.0, 0.0),
85
168
        m_combined_ang_vel (0.0, 0.0, 0.0),
86
169
        m_fh_object(0),
87
170
        m_inv_mass(0.0),
88
 
        m_inv_inertia(0., 0., 0.)
 
171
        m_inv_inertia(0., 0., 0.),
 
172
        m_kinematic(false),
 
173
        m_prev_kinematic(false),
 
174
        m_is_rigid_body(false),
 
175
        m_static(0)
89
176
{
 
177
        m_object = DT_CreateObject(this, shape);
90
178
        m_xform.setIdentity();
91
179
        m_xform.getValue(m_ogl_matrix);
92
180
        if (shapeProps)
104
192
        m_suspended = false;
105
193
}
106
194
 
 
195
 
107
196
        void 
108
197
SM_Object::
109
198
integrateForces(
110
199
        MT_Scalar timeStep
111
200
){
112
201
        if (!m_suspended) {
113
 
                m_prev_state = *this;
 
202
                m_prev_state = getNextFrame();
114
203
                m_prev_state.setLinearVelocity(actualLinVelocity());
115
204
                m_prev_state.setAngularVelocity(actualAngVelocity());
116
205
                if (isDynamic()) {
121
210
                        m_lin_mom *= pow(m_shapeProps->m_lin_drag, timeStep);
122
211
                        m_ang_mom *= pow(m_shapeProps->m_ang_drag, timeStep);
123
212
                        // Set velocities according momentum
124
 
                        m_lin_vel = m_lin_mom * m_inv_mass;
125
 
                        m_ang_vel = m_inv_inertia_tensor * m_ang_mom;
 
213
                        getNextFrame().setLinearVelocity(m_lin_mom * m_inv_mass);
 
214
                        getNextFrame().setAngularVelocity(m_inv_inertia_tensor * m_ang_mom);
126
215
                }
127
216
        }       
128
217
 
148
237
//#define BACKWARD
149
238
#ifdef  MIDPOINT
150
239
// Midpoint rule
151
 
                m_pos += (m_prev_state.getLinearVelocity() + actualLinVelocity()) * (timeStep * 0.5);
152
 
                m_orn += (m_prev_state.getAngularVelocity() * m_prev_state.getOrientation() + actualAngVelocity() * m_orn) * (timeStep * 0.25);
 
240
                getNextFrame().integrateMidpoint(timeStep, m_prev_state, actualLinVelocity(), actualAngVelocity());
153
241
#elif defined BACKWARD
154
242
// Backward Euler
155
 
                m_pos += actualLinVelocity() * timeStep;
156
 
                m_orn += actualAngVelocity() * m_orn * (timeStep * 0.5);
 
243
                getNextFrame().integrateBackward(timeStep, actualLinVelocity(), actualAngVelocity());
157
244
#else 
158
245
// Forward Euler
159
 
 
160
 
                m_pos += m_prev_state.getLinearVelocity() * timeStep;
161
 
                m_orn += m_prev_state.getAngularVelocity() * m_orn * (timeStep * 0.5);
 
246
                getNextFrame().integrateForward(timeStep, m_prev_state);
162
247
#endif
163
 
                m_orn.normalize(); // I might not be necessary to do this every call
164
248
 
165
249
                calcXform();
166
250
                notifyClient();         
196
280
        /**
197
281
         * if rel_vel_normal > 0, the objects are moving apart! 
198
282
         */
199
 
        if (rel_vel_normal < 0.) {
 
283
        if (rel_vel_normal < -MT_EPSILON) {
200
284
                /**
201
285
                 * if rel_vel_normal < ImpulseThreshold, scale the restitution down.
202
286
                 * This should improve the simulation where the object is stacked.
203
 
                 */
 
287
                 */     
204
288
                restitution *= MT_min(MT_Scalar(1.0), rel_vel_normal/ImpulseThreshold);
205
289
                                
206
290
                MT_Scalar impulse = -(1.0 + restitution) * rel_vel_normal;
214
298
                         * Apply impulse at the collision point.
215
299
                         * Take rotational inertia into account.
216
300
                         */
217
 
                        applyImpulse(local2 + m_pos, impulse * normal);
 
301
                        applyImpulse(local2 + getNextFrame().getPosition(), impulse * normal);
218
302
                } else {
219
303
                        /**
220
304
                         * Apply impulse through object centre. (no rotation.)
251
335
                         * local coordinates.
252
336
                         */
253
337
 
254
 
                        MT_Matrix3x3 lcs(m_orn);
 
338
                        MT_Matrix3x3 lcs(getNextFrame().getOrientation());
255
339
                        
256
340
                        /**
257
341
                         * We cannot use m_xform.getBasis() for the matrix, since 
303
387
 
304
388
                        // I guess the GEN_max is not necessary, so let's check it
305
389
 
306
 
                        assert(impulse >= 0.0);
 
390
                        MT_assert(impulse >= 0.0);
307
391
 
308
392
                        /**
309
393
                         * Here's the trick. We compute the impulse to make the
325
409
                                        (invMass + lateral.dot(temp.cross(local2)));
326
410
 
327
411
                                MT_Scalar friction = MT_min(impulse_lateral, max_friction);
328
 
                                applyImpulse(local2 + m_pos, -lateral * friction);
 
412
                                applyImpulse(local2 + getNextFrame().getPosition(), -lateral * friction);
329
413
                        }
330
414
                        else {
331
415
                                MT_Scalar impulse_lateral = rel_vel_lateral / invMass;
343
427
        }
344
428
}
345
429
 
 
430
static void AddCallback(SM_Scene *scene, SM_Object *obj1, SM_Object *obj2)
 
431
{
 
432
        // If we have callbacks on either of the client objects, do a collision test
 
433
        // and add a callback if they intersect.
 
434
        DT_Vector3 v;
 
435
        if ((obj1->getClientObject() && obj1->getClientObject()->hasCollisionCallback()) || 
 
436
            (obj2->getClientObject() && obj2->getClientObject()->hasCollisionCallback()) &&
 
437
             DT_GetIntersect(obj1->getObjectHandle(), obj2->getObjectHandle(), v))
 
438
                scene->notifyCollision(obj1, obj2);
 
439
}
 
440
 
346
441
DT_Bool SM_Object::boing(
347
442
        void *client_data,  
348
443
        void *object1,
349
444
        void *object2,
350
445
        const DT_CollData *coll_data
351
446
){
352
 
        //if (!coll_data)
353
 
        //      return DT_CONTINUE;
354
 
 
355
447
        SM_Scene  *scene = (SM_Scene *)client_data; 
356
448
        SM_Object *obj1  = (SM_Object *)object1;  
357
449
        SM_Object *obj2  = (SM_Object *)object2;  
358
450
        
359
 
        scene->addPair(obj1, obj2); // Record this collision for client callbacks
 
451
        // at this point it is unknown whether we are really intersecting (broad phase)
 
452
        
 
453
        DT_Vector3 p1, p2;
 
454
        if (!obj2->isDynamic()) {
 
455
                std::swap(obj1, obj2);
 
456
        }
360
457
        
361
458
        // If one of the objects is a ghost then ignore it for the dynamics
362
459
        if (obj1->isGhost() || obj2->isGhost()) {
 
460
                AddCallback(scene, obj1, obj2);
363
461
                return DT_CONTINUE;
364
462
        }
365
463
 
366
464
        // Objects do not collide with parent objects
367
465
        if (obj1->getDynamicParent() == obj2 || obj2->getDynamicParent() == obj1) {
 
466
                AddCallback(scene, obj1, obj2);
368
467
                return DT_CONTINUE;
369
468
        }
370
469
        
371
470
        if (!obj2->isDynamic()) {
372
 
                std::swap(obj1, obj2);
373
 
        }
374
 
 
375
 
        if (!obj2->isDynamic()) {
 
471
                AddCallback(scene, obj1, obj2);
376
472
                return DT_CONTINUE;
377
473
        }
378
474
 
379
475
        // Get collision data from SOLID
380
 
        DT_Vector3 p1, p2;
381
476
        if (!DT_GetPenDepth(obj1->getObjectHandle(), obj2->getObjectHandle(), p1, p2))
382
477
                return DT_CONTINUE;
 
478
        
383
479
        MT_Point3 local1(p1), local2(p2);
384
480
        MT_Vector3 normal(local2 - local1);
385
481
        MT_Scalar dist = normal.length();
386
482
        
387
483
        if (dist < MT_EPSILON)
388
484
                return DT_CONTINUE;
 
485
                
 
486
        // Now we are definitely intersecting.
 
487
 
 
488
        // Set callbacks for game engine.
 
489
        if ((obj1->getClientObject() && obj1->getClientObject()->hasCollisionCallback()) || 
 
490
            (obj2->getClientObject() && obj2->getClientObject()->hasCollisionCallback()))
 
491
                scene->notifyCollision(obj1, obj2);
389
492
        
390
 
        local1 -= obj1->m_pos;
391
 
        local2 -= obj2->m_pos;
 
493
        local1 -= obj1->getNextFrame().getPosition();
 
494
        local2 -= obj2->getNextFrame().getPosition();
392
495
        
393
496
        // Calculate collision parameters
394
497
        MT_Vector3 rel_vel        = obj1->getVelocity(local1) - obj2->getVelocity(local2);
410
513
                obj1->dynamicCollision(local1, normal, dist, rel_vel, restitution, friction_factor, invMass);
411
514
                
412
515
        if (obj2->isDynamic())
 
516
        {
413
517
                obj2->dynamicCollision(local2, -normal, dist, -rel_vel, restitution, friction_factor, invMass);
 
518
                if (!obj1->isDynamic() || obj1->m_static)
 
519
                        obj2->m_static = obj1->m_static + 1;
 
520
        }
414
521
        
415
522
        return DT_CONTINUE;
416
523
}
421
528
        void *object2,
422
529
        const DT_CollData *coll_data
423
530
){
424
 
        SM_Scene  *scene = (SM_Scene *)client_data; 
425
531
        SM_Object *obj1  = (SM_Object *)object1;  
426
532
        SM_Object *obj2  = (SM_Object *)object2;  
427
533
        
450
556
        // Get collision data from SOLID
451
557
        MT_Vector3 normal(local2 - local1);
452
558
        
453
 
        if (normal.dot(normal) < MT_EPSILON)
 
559
        MT_Scalar dist = normal.dot(normal);
 
560
        if (dist < MT_EPSILON || dist > obj2->m_shapeProps->m_radius*obj2->m_shapeProps->m_radius)
454
561
                return DT_CONTINUE;
455
562
                
456
 
        // This distinction between dynamic and non-dynamic objects should not be 
457
 
        // necessary. Non-dynamic objects are assumed to have infinite mass.
458
 
        if (obj1->isDynamic()) {
459
 
                MT_Vector3 error = normal * 0.5f;
460
 
                obj1->m_error += error;
461
 
                obj2->m_error -= error;
462
 
                // Remove the velocity component in the normal direction
463
 
                // Calculate collision parameters
464
 
                /*MT_Vector3 rel_vel = obj1->getLinearVelocity() - obj2->getLinearVelocity();
465
 
                if (normal.length() > FixThreshold && rel_vel.length() < FixVelocity) {
466
 
                        normal.normalize();
467
 
                        MT_Scalar  rel_vel_normal = 0.49*(normal.dot(rel_vel));
468
 
 
469
 
                        obj1->addLinearVelocity(-rel_vel_normal*normal);
470
 
                        obj2->addLinearVelocity(rel_vel_normal*normal);
471
 
                }*/
472
 
        }
473
 
        else {
474
 
                // Same again but now obj1 is non-dynamic
475
 
                obj2->m_error -= normal;
476
 
                /*MT_Vector3 rel_vel = obj2->getLinearVelocity();
477
 
                if (normal.length() > FixThreshold && rel_vel.length() < FixVelocity) {
478
 
                        // Calculate collision parameters
479
 
                        normal.normalize();
480
 
                        MT_Scalar  rel_vel_normal = -0.99*(normal.dot(rel_vel));
481
 
 
482
 
                        obj2->addLinearVelocity(rel_vel_normal*normal);
483
 
                }*/ 
484
 
        }
 
563
                
 
564
        if ((obj1->m_static || !obj1->isDynamic()) && obj1->m_static < obj2->m_static)
 
565
        {
 
566
                obj2->m_static = obj1->m_static + 1;
 
567
        } else if (obj2->m_static && obj2->m_static < obj1->m_static)
 
568
        {
 
569
                obj1->m_static = obj2->m_static + 1;
 
570
        }
 
571
        
 
572
        contacts.insert(new Contact(obj1, obj2, normal, MT_Point3(local1 + 0.5*(local2 - local1))));
 
573
        
485
574
        
486
575
        return DT_CONTINUE;
487
576
}
488
577
 
489
578
void SM_Object::relax(void)
490
 
{ 
 
579
{
 
580
        for (Contact::Set::iterator csit = contacts.begin() ; csit != contacts.end(); ++csit)
 
581
        {
 
582
                (*csit)->resolve();
 
583
                delete (*csit);
 
584
        }
 
585
                
 
586
        contacts.clear();
491
587
        if (m_error.fuzzyZero())
492
588
                return;
493
589
        //std::cout << "SM_Object::relax: { " << m_error << " }" << std::endl;
494
590
        
495
 
        m_pos += m_error; 
 
591
        getNextFrame().setPosition(getNextFrame().getPosition() + m_error); 
496
592
        m_error.setValue(0., 0., 0.); 
497
 
/*      m_pos.getValue(pos);
498
 
        DT_SetPosition(m_object, pos); 
499
 
        m_xform.setOrigin(m_pos); 
500
 
        m_xform.getValue(m_ogl_matrix); */
501
 
        calcXform();
502
 
        notifyClient();
 
593
        //calcXform();
 
594
        //notifyClient();
503
595
}
504
596
        
505
597
SM_Object::SM_Object() :
506
598
        m_dynamicParent(0),
507
599
        m_client_object(0),
508
 
        
 
600
        m_physicsClientObject(0),
509
601
        m_shape(0),
510
602
        m_materialProps(0),
511
603
        m_materialPropsBackup(0),
516
608
        m_scaling(1.0, 1.0, 1.0),
517
609
        m_reaction_impulse(0.0, 0.0, 0.0),
518
610
        m_reaction_force(0.0, 0.0, 0.0),
519
 
        m_kinematic(false),
520
 
        m_prev_kinematic(false),
521
 
        m_is_rigid_body(false),
522
611
        m_lin_mom(0.0, 0.0, 0.0),
523
612
        m_ang_mom(0.0, 0.0, 0.0),
524
613
        m_force(0.0, 0.0, 0.0),
526
615
        m_error(0.0, 0.0, 0.0),
527
616
        m_combined_lin_vel (0.0, 0.0, 0.0),
528
617
        m_combined_ang_vel (0.0, 0.0, 0.0),
529
 
        m_fh_object(0) 
 
618
        m_fh_object(0),
 
619
        m_kinematic(false),
 
620
        m_prev_kinematic(false),
 
621
        m_is_rigid_body(false)
530
622
{
531
623
        // warning no initialization of variables done by moto.
532
624
}
645
737
        printf("                 m_scaling = { %-0.5f, %-0.5f, %-0.5f }\n",
646
738
                m_scaling[0], m_scaling[1], m_scaling[2]);
647
739
#endif
648
 
        m_xform.setOrigin(m_pos);
649
 
        m_xform.setBasis(MT_Matrix3x3(m_orn, m_scaling));
 
740
        m_xform.setOrigin(getNextFrame().getPosition());
 
741
        m_xform.setBasis(MT_Matrix3x3(getNextFrame().getOrientation(), m_scaling));
650
742
        m_xform.getValue(m_ogl_matrix);
651
743
        
652
744
        /* Blender has been known to crash here.
653
745
           This usually means SM_Object *this has been deleted more than once. */
654
746
        DT_SetMatrixd(m_object, m_ogl_matrix);
655
747
        if (m_fh_object) {
656
 
                m_fh_object->setPosition(m_pos);
 
748
                m_fh_object->setPosition(getNextFrame().getPosition());
657
749
                m_fh_object->calcXform();
658
750
        }
659
751
        updateInvInertiaTensor();
769
861
        const MT_Point3& pos
770
862
){
771
863
        m_kinematic = true;
772
 
        m_pos = pos;
 
864
        getNextFrame().setPosition(pos);
 
865
        endFrame();
773
866
}
774
 
 
 
867
        
775
868
        void 
776
869
SM_Object::
777
870
setOrientation(
778
871
        const MT_Quaternion& orn
779
872
){
780
 
        assert(!orn.fuzzyZero());
 
873
        MT_assert(!orn.fuzzyZero());
781
874
        m_kinematic = true;
782
 
        m_orn = orn;
 
875
        getNextFrame().setOrientation(orn);
 
876
        endFrame();
783
877
}
784
878
 
785
879
        void 
816
910
addLinearVelocity(
817
911
        const MT_Vector3& lin_vel
818
912
){
819
 
        m_lin_vel += lin_vel;
820
 
        if (m_shapeProps) {
821
 
                m_lin_mom = m_lin_vel * m_shapeProps->m_mass;
822
 
        }
 
913
        setLinearVelocity(getNextFrame().getLinearVelocity() + lin_vel);
823
914
}
824
915
 
825
916
        void 
827
918
setLinearVelocity(
828
919
        const MT_Vector3& lin_vel
829
920
){
830
 
        m_lin_vel = lin_vel;
 
921
        getNextFrame().setLinearVelocity(lin_vel);
831
922
        if (m_shapeProps) {
832
 
                m_lin_mom = m_lin_vel * m_shapeProps->m_mass;
 
923
                m_lin_mom = getNextFrame().getLinearVelocity() * m_shapeProps->m_mass;
833
924
        }
834
925
}
835
926
 
858
949
setAngularVelocity(
859
950
        const MT_Vector3& ang_vel
860
951
) {
861
 
        m_ang_vel = ang_vel;
 
952
        getNextFrame().setAngularVelocity(ang_vel);
862
953
        if (m_shapeProps) {
863
 
                m_ang_mom = m_ang_vel * m_shapeProps->m_inertia;
 
954
                m_ang_mom = getNextFrame().getAngularVelocity() * m_shapeProps->m_inertia;
864
955
        }
865
956
}
866
957
 
869
960
addAngularVelocity(
870
961
        const MT_Vector3& ang_vel
871
962
) {
872
 
        m_ang_vel += ang_vel;
873
 
        if (m_shapeProps) {
874
 
                m_ang_mom = m_ang_vel * m_shapeProps->m_inertia;
875
 
        }
 
963
        setAngularVelocity(getNextFrame().getAngularVelocity() + ang_vel);
876
964
}
877
965
 
878
966
 
905
993
        if (isDynamic()) {              
906
994
 
907
995
#if 1
908
 
                m_lin_vel += lin_vel;
909
 
                m_ang_vel += ang_vel;
 
996
                getNextFrame().setLinearVelocity(getNextFrame().getLinearVelocity() + lin_vel);
 
997
                getNextFrame().setAngularVelocity(getNextFrame().getAngularVelocity() + ang_vel);
910
998
#else
911
999
 
912
1000
                //compute the component of the physics velocity in the 
913
1001
                // direction of the set velocity and set it to zero.
914
1002
                MT_Vector3 lin_vel_norm = lin_vel.normalized();
915
1003
 
916
 
                m_lin_vel -= (m_lin_vel.dot(lin_vel_norm) * lin_vel_norm);
 
1004
                setLinearVelocity(getNextFrame().getLinearVelocity() - (getNextFrame().getLinearVelocity().dot(lin_vel_norm) * lin_vel_norm));
917
1005
#endif
918
 
                m_lin_mom = m_lin_vel * m_shapeProps->m_mass;
919
 
                m_ang_mom = m_ang_vel * m_shapeProps->m_inertia;
 
1006
                m_lin_mom = getNextFrame().getLinearVelocity() * m_shapeProps->m_mass;
 
1007
                m_ang_mom = getNextFrame().getAngularVelocity() * m_shapeProps->m_inertia;
920
1008
                clearCombinedVelocities();
921
1009
 
922
1010
        }
979
1067
        const MT_Point3& attach, const MT_Vector3& impulse
980
1068
) {
981
1069
        applyCenterImpulse(impulse);                          // Change in linear momentum
982
 
        applyAngularImpulse((attach - m_pos).cross(impulse)); // Change in angular momentump
 
1070
        applyAngularImpulse((attach - getNextFrame().getPosition()).cross(impulse)); // Change in angular momentump
983
1071
}
984
1072
 
985
1073
        void 
990
1078
        if (m_shapeProps) {
991
1079
                m_lin_mom          += impulse;
992
1080
                m_reaction_impulse += impulse;
993
 
                m_lin_vel           = m_lin_mom * m_inv_mass;
 
1081
                getNextFrame().setLinearVelocity(m_lin_mom * m_inv_mass);
994
1082
 
995
1083
                // The linear velocity is immedialtely updated since otherwise
996
1084
                // simultaneous collisions will get a double impulse. 
1004
1092
) {
1005
1093
        if (m_shapeProps) {
1006
1094
                m_ang_mom += impulse;
1007
 
                m_ang_vel = m_inv_inertia_tensor * m_ang_mom;
 
1095
                getNextFrame().setAngularVelocity( m_inv_inertia_tensor * m_ang_mom);
1008
1096
        }
1009
1097
}
1010
1098
 
1108
1196
SM_Object::
1109
1197
actualLinVelocity(
1110
1198
) const {
1111
 
        return m_combined_lin_vel + m_lin_vel;
 
1199
        return m_combined_lin_vel + getNextFrame().getLinearVelocity();
1112
1200
};
1113
1201
 
1114
1202
const 
1116
1204
SM_Object::
1117
1205
actualAngVelocity(
1118
1206
) const {
1119
 
        return m_combined_ang_vel + m_ang_vel;
1120
 
};
1121
 
 
1122
 
 
1123
 
 
1124
 
 
1125
 
 
 
1207
        return m_combined_ang_vel + getNextFrame().getAngularVelocity();
 
1208
}
 
1209
 
 
1210
 
 
1211
SM_MotionState&
 
1212
SM_Object::
 
1213
getCurrentFrame()
 
1214
{
 
1215
        return m_frames[1];
 
1216
}
 
1217
 
 
1218
SM_MotionState&
 
1219
SM_Object::
 
1220
getPreviousFrame()
 
1221
{
 
1222
        return m_frames[0];
 
1223
}
 
1224
 
 
1225
SM_MotionState &
 
1226
SM_Object::
 
1227
getNextFrame()
 
1228
{
 
1229
        return m_frames[2];
 
1230
}
 
1231
 
 
1232
const SM_MotionState &
 
1233
SM_Object::
 
1234
getCurrentFrame() const
 
1235
{
 
1236
        return m_frames[1];
 
1237
}
 
1238
 
 
1239
const SM_MotionState &
 
1240
SM_Object::
 
1241
getPreviousFrame() const
 
1242
{
 
1243
        return m_frames[0];
 
1244
}
 
1245
 
 
1246
const SM_MotionState &
 
1247
SM_Object::
 
1248
getNextFrame() const
 
1249
{
 
1250
        return m_frames[2];
 
1251
}
 
1252
 
 
1253
 
 
1254
const MT_Point3&     
 
1255
SM_Object::
 
1256
getPosition()        const
 
1257
{
 
1258
        return m_frames[1].getPosition();
 
1259
}
 
1260
 
 
1261
const MT_Quaternion& 
 
1262
SM_Object::
 
1263
getOrientation()     const
 
1264
{
 
1265
        return m_frames[1].getOrientation();
 
1266
}
 
1267
 
 
1268
const MT_Vector3&    
 
1269
SM_Object::
 
1270
getLinearVelocity()  const
 
1271
{
 
1272
        return m_frames[1].getLinearVelocity();
 
1273
}
 
1274
 
 
1275
const MT_Vector3&    
 
1276
SM_Object::
 
1277
getAngularVelocity() const
 
1278
{
 
1279
        return m_frames[1].getAngularVelocity();
 
1280
}
 
1281
 
 
1282
void
 
1283
SM_Object::
 
1284
interpolate(MT_Scalar timeStep)
 
1285
{
 
1286
        if (!actualLinVelocity().fuzzyZero() || !actualAngVelocity().fuzzyZero()) 
 
1287
        {
 
1288
                getCurrentFrame().setTime(timeStep);
 
1289
                getCurrentFrame().lerp(getPreviousFrame(), getNextFrame());
 
1290
                notifyClient();
 
1291
        }
 
1292
}
 
1293
 
 
1294
void
 
1295
SM_Object::
 
1296
endFrame()
 
1297
{
 
1298
        getPreviousFrame() = getNextFrame();
 
1299
        getCurrentFrame() = getNextFrame();
 
1300
        m_static = 0;
 
1301
}