neo/game/physics/Force_Spring.cpp

   1 /*
   2 ===========================================================================
   3
   4 Doom 3 GPL Source Code
   5 Copyright (C) 1999-2011 id Software LLC, a ZeniMax Media company.
   6
   7 This file is part of the Doom 3 GPL Source Code (?Doom 3 Source Code?).
   8
   9 Doom 3 Source Code is free software: you can redistribute it and/or modify
  10 it under the terms of the GNU General Public License as published by
  11 the Free Software Foundation, either version 3 of the License, or
  12 (at your option) any later version.
  13
  14 Doom 3 Source Code is distributed in the hope that it will be useful,
  15 but WITHOUT ANY WARRANTY; without even the implied warranty of
  16 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  17 GNU General Public License for more details.
  18
  19 You should have received a copy of the GNU General Public License
  20 along with Doom 3 Source Code.  If not, see <http://www.gnu.org/licenses/>.
  21
  22 In addition, the Doom 3 Source Code is also subject to certain additional terms. You should have received a copy of these additional terms immediately following the terms and conditions of the GNU General Public License which accompanied the Doom 3 Source Code.  If not, please request a copy in writing from id Software at the address below.
  23
  24 If you have questions concerning this license or the applicable additional terms, you may contact in writing id Software LLC, c/o ZeniMax Media Inc., Suite 120, Rockville, Maryland 20850 USA.
  25
  26 ===========================================================================
  27 */
  28
  29 #include "../../idlib/precompiled.h"
  30 #pragma hdrstop
  31
  32 #include "../Game_local.h"
  33
  34 CLASS_DECLARATION( idForce, idForce_Spring )
  35 END_CLASS
  36
  37 /*
  38 ================
  39 idForce_Spring::idForce_Spring
  40 ================
  41 */
  42 idForce_Spring::idForce_Spring( void ) {
  43         Kstretch                = 100.0f;
  44         Kcompress               = 100.0f;
  45         damping                 = 0.0f;
  46         restLength              = 0.0f;
  47         physics1                = NULL;
  48         id1                             = 0;
  49         p1                              = vec3_zero;
  50         physics2                = NULL;
  51         id2                             = 0;
  52         p2                              = vec3_zero;
  53 }
  54
  55 /*
  56 ================
  57 idForce_Spring::~idForce_Spring
  58 ================
  59 */
  60 idForce_Spring::~idForce_Spring( void ) {
  61 }
  62
  63 /*
  64 ================
  65 idForce_Spring::InitSpring
  66 ================
  67 */
  68 void idForce_Spring::InitSpring( float Kstretch, float Kcompress, float damping, float restLength ) {
  69         this->Kstretch = Kstretch;
  70         this->Kcompress = Kcompress;
  71         this->damping = damping;
  72         this->restLength = restLength;
  73 }
  74
  75 /*
  76 ================
  77 idForce_Spring::SetPosition
  78 ================
  79 */
  80 void idForce_Spring::SetPosition( idPhysics *physics1, int id1, const idVec3 &p1, idPhysics *physics2, int id2, const idVec3 &p2 ) {
  81         this->physics1 = physics1;
  82         this->id1 = id1;
  83         this->p1 = p1;
  84         this->physics2 = physics2;
  85         this->id2 = id2;
  86         this->p2 = p2;
  87 }
  88
  89 /*
  90 ================
  91 idForce_Spring::Evaluate
  92 ================
  93 */
  94 void idForce_Spring::Evaluate( int time ) {
  95         float length;
  96         idMat3 axis;
  97         idVec3 pos1, pos2, velocity1, velocity2, force, dampingForce;
  98         impactInfo_t info;
  99
 100         pos1 = p1;
 101         pos2 = p2;
 102         velocity1 = velocity2 = vec3_origin;
 103
 104         if ( physics1 ) {
 105                 axis = physics1->GetAxis( id1 );
 106                 pos1 = physics1->GetOrigin( id1 );
 107                 pos1 += p1 * axis;
 108                 if ( damping > 0.0f ) {
 109                         physics1->GetImpactInfo( id1, pos1, &info );
 110                         velocity1 = info.velocity;
 111                 }
 112         }
 113
 114         if ( physics2 ) {
 115                 axis = physics2->GetAxis( id2 );
 116                 pos2 = physics2->GetOrigin( id2 );
 117                 pos2 += p2 * axis;
 118                 if ( damping > 0.0f ) {
 119                         physics2->GetImpactInfo( id2, pos2, &info );
 120                         velocity2 = info.velocity;
 121                 }
 122         }
 123
 124         force = pos2 - pos1;
 125         dampingForce = ( damping * ( ((velocity2 - velocity1) * force) / (force * force) ) ) * force;
 126         length = force.Normalize();
 127
 128         // if the spring is stretched
 129         if ( length > restLength ) {
 130                 if ( Kstretch > 0.0f ) {
 131                         force = ( Square( length - restLength ) * Kstretch ) * force - dampingForce;
 132                         if ( physics1 ) {
 133                                 physics1->AddForce( id1, pos1, force );
 134                         }
 135                         if ( physics2 ) {
 136                                 physics2->AddForce( id2, pos2, -force );
 137                         }
 138                 }
 139         }
 140         else {
 141                 if ( Kcompress > 0.0f ) {
 142                         force = ( Square( length - restLength ) * Kcompress ) * force - dampingForce;
 143                         if ( physics1 ) {
 144                                 physics1->AddForce( id1, pos1, -force );
 145                         }
 146                         if ( physics2 ) {
 147                                 physics2->AddForce( id2, pos2, force );
 148                         }
 149                 }
 150         }
 151 }
 152
 153 /*
 154 ================
 155 idForce_Spring::RemovePhysics
 156 ================
 157 */
 158 void idForce_Spring::RemovePhysics( const idPhysics *phys ) {
 159         if ( physics1 == phys ) {
 160                 physics1 = NULL;
 161         }
 162         if ( physics2 == phys ) {
 163                 physics2 = NULL;
 164         }
 165 }