src/phys/player.cpp

   1 /************************************************************************
   2     This file is part of NE.
   3
   4     NE is free software: you can redistribute it and/or modify
   5     it under the terms of the GNU General Public License as published by
   6     the Free Software Foundation, either version 3 of the License, or
   7     (at your option) any later version.
   8
   9     NE is distributed in the hope that it will be useful,
  10     but WITHOUT ANY WARRANTY; without even the implied warranty of
  11     MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12     GNU General Public License for more details.
  13
  14     You should have received a copy of the GNU General Public License
  15     along with NE.  If not, see <http://www.gnu.org/licenses/>.
  16 ************************************************************************/
  17
  18 #include "player.h"
  19
  20 #include "world.h"
  21 #include "backend/shapes.h"
  22 #include "base/quat.h"
  23
  24 #include <ode/ode.h>
  25 #include <GL/gl.h>
  26 #include <GL/glu.h>
  27
  28 Player::Player(float height, float density) : m_standing(false)
  29 {
  30         float radius, length;
  31         radius = height * 0.618;
  32         length = height * 0.85;
  33
  34         World *w = World::Instance();
  35         m_capsule = new dCapsule(w->getSpace()->id(), radius, length);
  36         m_geom = m_capsule;
  37
  38         m_ray = new dRay(0, getBodyHeight() - length);
  39         m_ray->setParams(0, 1);
  40         m_ray->setClosestHit(1);
  41
  42         m_mass.setCapsule(density, 2, radius, length);
  43         m_body->setMass(&m_mass);
  44
  45         init_geom();
  46         shapes_init();
  47 }
  48
  49 Player::~Player()
  50 {
  51         delete m_capsule;
  52         delete m_ray;
  53 }
  54
  55 void Player::draw()
  56 {
  57         glPushMatrix();
  58         apply_matrix();
  59
  60         dReal radius, length;
  61         m_capsule->getParams(&radius, &length);
  62
  63         if (m_standing)
  64                 glColor3f(0,0,1);
  65         else
  66                 glColor3f(1,1,1);
  67
  68         glScalef(radius, length, radius);
  69         draw_cube();
  70         glPopMatrix();
  71
  72         static GLUquadricObj *q = 0;
  73         if (q == 0)
  74         {
  75                 q = gluNewQuadric();
  76                 gluQuadricNormals(q, GL_SMOOTH);
  77                 gluQuadricTexture(q, GL_TRUE);
  78                 glTexGeni(GL_S, GL_TEXTURE_GEN_MODE, GL_SPHERE_MAP);
  79                 glTexGeni(GL_T, GL_TEXTURE_GEN_MODE, GL_SPHERE_MAP);
  80         }
  81
  82         float hr = radius * 0.5;
  83
  84         glPushMatrix();
  85         apply_matrix();
  86         glTranslatef(0.0, length*0.5, 0.0);
  87         glScalef(hr, hr, hr);
  88         gluSphere(q,1,16,16);
  89         glPopMatrix();
  90
  91         glPushMatrix();
  92         apply_matrix();
  93         glTranslatef(0.0, -length*0.5, 0.0);
  94         glScalef(hr, hr, hr);
  95         gluSphere(q,1,16,16);
  96         glPopMatrix();
  97
  98         if (m_standing)
  99                 glColor3f(0,1,0);
 100         else
 101                 glColor3f(1,1,0);
 102
 103         Vec3f a = getBottom();
 104         Vec3f b = getFeet();
 105
 106         glDisable(GL_LIGHTING);
 107         glBegin(GL_LINES);
 108         glVertex3f(a.x(), a.y(), a.z());
 109         glVertex3f(b.x(), b.y(), b.z());
 110         glEnd();
 111         glEnable(GL_LIGHTING);
 112 }
 113
 114 void Player::update()
 115 {
 116         update_ray();
 117
 118         World *w = World::Instance();
 119
 120         dReal depth = w->trace(m_ray);
 121
 122         if (depth != dInfinity) {
 123                 m_standing = true;
 124                 m_depth = depth;
 125
 126                 Vec3f g = -m_gravity;
 127
 128                 printf("%.2f, %.2f, %.2f\n", g.x(), g.y(), g.z());
 129
 130                 Quatf to = Quatf(m_body->getQuaternion()[0], -m_gravity.unit());
 131                 printf("%.2f, %.2f, %.2f, %.2f\n", to.w(), to.x(), to.y(), to.z());
 132                 to = to.unit();
 133                 printf("%.2f, %.2f, %.2f, %.2f\n", to.w(), to.x(), to.y(), to.z());
 134
 135                 to.v().draw(getPositionV(), 100);
 136
 137                 m_body->setQuaternion(to.toODE());
 138                 m_body->setAngularVel(0.0, 0.0, 0.0);
 139                 m_body->setTorque(0.0, 0.0, 0.0);
 140
 141                 dReal x = m_ray->getLength() - m_depth;
 142
 143                 dReal k = m_mass.mass * 2;
 144
 145                 Vec3f vel = Vec3f::FromODE(m_body->getLinearVel());
 146                 vel *= w->getStepSize() * w->getStepCount();
 147
 148                 dReal vdamp = vel.dot(-getUp());
 149
 150                 dReal force = k * x + k * vdamp;
 151
 152                 printf("------------------\n");
 153                 printf("x = %.2f    k = %.2f\n", x, k);
 154                 printf("applying %.2f force\n", force);
 155
 156                 m_body->addRelForce(0.0, force, 0.0);
 157
 158                 //m_body->addTorque(ang_damp.x(), ang_damp.y(), ang_damp.z());
 159
 160                 // do damping
 161                 dReal damp = -0.25;
 162
 163                 vel *= damp;
 164                 //m_body->addForce(vel.x(), vel.y(), vel.z());
 165
 166                 //Vec3f tor = Vec3f::FromODE(m_body->getTorque()) * damp;
 167
 168                 //m_body->addTorque(tor.x(), tor.y(), tor.z());
 169         }
 170         else {
 171                 Vec3f norm = getUp().unit();
 172                 Vec3f grav = m_gravity.unit();
 173
 174                 float tip = 1.0 - norm.dot(grav);
 175
 176                 Quatf from = Quatf::FromODE(getQuaternion());
 177
 178                 Quatf to = Quatf::FromAxis(from.angle(), grav);
 179                 Quatf q = from.slerp(to, 0.5);
 180
 181                 Vec3f eu = q.toEuler() * tip;
 182                 getBody()->addTorque(eu.x(), eu.y(), eu.z());
 183         }
 184 }
 185
 186 float Player::getBodyHeight()
 187 {
 188         dReal length, radius;
 189         m_capsule->getParams(&radius, &length);
 190
 191         return length + 2 * radius;
 192 }
 193
 194 const Vec3f Player::getFeet()
 195 {
 196         dReal dist = m_ray->getLength();
 197         Quatf quat = Quatf::FromODE(getQuaternion());
 198
 199         return getBottom() + getUp() * -dist;
 200 }
 201
 202 const Vec3f Player::getBottom()
 203 {
 204         Vec3f pos = getPositionV();
 205         Quatf quat = Quatf::FromODE(getQuaternion());
 206
 207         dReal radius, length;
 208         m_capsule->getParams(&radius, &length);
 209
 210         length *= 0.5;
 211
 212         Vec3f offset = getUp() * -length;
 213
 214         return pos + offset;
 215 }
 216
 217 void Player::update_ray()
 218 {
 219         Vec3f bottom = getBottom();
 220         Vec3f down = -getUp();
 221
 222         m_ray->set(bottom.x(), bottom.y(), bottom.z(),
 223                            down.x(), down.y(), down.z());
 224 }
 225