source/gameengine/Physics/Sumo/Fuzzics/src/SM_FhObject.cpp

   1 /**
   2  * $Id$
   3  *
   4  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
   5  *
   6  * This program is free software; you can redistribute it and/or
   7  * modify it under the terms of the GNU General Public License
   8  * as published by the Free Software Foundation; either version 2
   9  * of the License, or (at your option) any later version. The Blender
  10  * Foundation also sells licenses for use in proprietary software under
  11  * the Blender License.  See http://www.blender.org/BL/ for information
  12  * about this.
  13  *
  14  * This program 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 this program; if not, write to the Free Software Foundation,
  21  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  22  *
  23  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
  24  * All rights reserved.
  25  *
  26  * The Original Code is: all of this file.
  27  *
  28  * Contributor(s): none yet.
  29  *
  30  * ***** END GPL/BL DUAL LICENSE BLOCK *****
  31  */
  32 #include "SM_FhObject.h"
  33 #include "MT_MinMax.h"
  34
  35 #ifdef HAVE_CONFIG_H
  36 #include <config.h>
  37 #endif
  38
  39 SM_FhObject::SM_FhObject(DT_ShapeHandle rayshape, MT_Vector3 ray, SM_Object *parent_object) :
  40                 SM_Object(rayshape, NULL, NULL, NULL),
  41                 m_ray(ray),
  42                 m_ray_direction(ray.normalized()),
  43                 m_parent_object(parent_object)
  44 {
  45 }
  46
  47 SM_FhObject::~SM_FhObject()
  48 {
  49         DT_DeleteShape(getShapeHandle());
  50 }
  51
  52 DT_Bool SM_FhObject::ray_hit(void *client_data,
  53                                                   void *client_object1,
  54                                                   void *client_object2,
  55                                                   const DT_CollData *coll_data)
  56 {
  57
  58         SM_FhObject *fh_object  = dynamic_cast<SM_FhObject *>((SM_Object *)client_object2);
  59         if (!fh_object)
  60         {
  61                 std::swap(client_object1, client_object2);
  62                 fh_object  = dynamic_cast<SM_FhObject *>((SM_Object *)client_object2);
  63         }
  64
  65         SM_Object   *hit_object = (SM_Object *)client_object1;
  66         const SM_MaterialProps *matProps = hit_object->getMaterialProps();
  67
  68         if ((matProps == 0) || (matProps->m_fh_distance < MT_EPSILON)) {
  69                 return DT_CONTINUE;
  70         }
  71
  72         SM_Object           *cl_object  = fh_object->getParentObject();
  73
  74         assert(fh_object);
  75
  76         if (hit_object == cl_object) {
  77                 // Shot myself in the foot...
  78                 return DT_CONTINUE;
  79         }
  80
  81         const SM_ShapeProps *shapeProps = cl_object->getShapeProps();
  82
  83         // Exit if the client object is not dynamic.
  84         if (shapeProps == 0) {
  85                 return DT_CONTINUE;
  86         }
  87
  88         MT_Point3 lspot;
  89         MT_Vector3 normal;
  90
  91         DT_Vector3 from, to, dnormal;
  92         DT_Scalar dlspot;
  93         fh_object->getPosition().getValue(from);
  94         fh_object->getSpot().getValue(to);
  95
  96
  97         if (DT_ObjectRayCast(hit_object->getObjectHandle(),
  98                                                  from,
  99                                                  to,
 100                                                  1.,
 101                                                  &dlspot,
 102                                                  dnormal)) {
 103
 104                 lspot = fh_object->getPosition() + (fh_object->getSpot() - fh_object->getPosition()) * dlspot;
 105                 const MT_Vector3& ray_dir = fh_object->getRayDirection();
 106                 MT_Scalar dist = MT_distance(fh_object->getPosition(), lspot) -
 107                         cl_object->getMargin() - shapeProps->m_radius;
 108
 109                 normal = MT_Vector3(dnormal).safe_normalized();
 110
 111                 if (dist < matProps->m_fh_distance) {
 112
 113                         if (shapeProps->m_do_fh) {
 114                                 lspot -= hit_object->getPosition();
 115                                 MT_Vector3 rel_vel = cl_object->getLinearVelocity() - hit_object->getVelocity(lspot);
 116                                 MT_Scalar rel_vel_ray = ray_dir.dot(rel_vel);
 117                                 MT_Scalar spring_extent = 1.0 - dist / matProps->m_fh_distance;
 118
 119                                 MT_Scalar i_spring = spring_extent * matProps->m_fh_spring;
 120                                 MT_Scalar i_damp =   rel_vel_ray * matProps->m_fh_damping;
 121
 122                                 cl_object->addLinearVelocity(-(i_spring + i_damp) * ray_dir);
 123                                 if (matProps->m_fh_normal) {
 124                                         cl_object->addLinearVelocity(
 125                                                 (i_spring + i_damp) *
 126                                                 (normal - normal.dot(ray_dir) * ray_dir));
 127                                 }
 128
 129                                 MT_Vector3 lateral = rel_vel - rel_vel_ray * ray_dir;
 130                                 const SM_ShapeProps *shapeProps = cl_object->getShapeProps();
 131
 132                                 if (shapeProps->m_do_anisotropic) {
 133                                         MT_Matrix3x3 lcs(cl_object->getOrientation());
 134                                         MT_Vector3 loc_lateral = lateral * lcs;
 135                                         const MT_Vector3& friction_scaling =
 136                                                 shapeProps->m_friction_scaling;
 137
 138                                         loc_lateral.scale(friction_scaling[0],
 139                                                                           friction_scaling[1],
 140                                                                           friction_scaling[2]);
 141                                         lateral = lcs * loc_lateral;
 142                                 }
 143
 144
 145                                 MT_Scalar rel_vel_lateral = lateral.length();
 146
 147                                 if (rel_vel_lateral > MT_EPSILON) {
 148                                         MT_Scalar friction_factor = matProps->m_friction;
 149                                         MT_Scalar max_friction = friction_factor * MT_max(MT_Scalar(0.0), i_spring);
 150
 151                                         MT_Scalar rel_mom_lateral = rel_vel_lateral /
 152                                                 cl_object->getInvMass();
 153
 154                                         MT_Vector3 friction =
 155                                                 (rel_mom_lateral > max_friction) ?
 156                                                 -lateral * (max_friction / rel_vel_lateral) :
 157                                                 -lateral;
 158
 159                                         cl_object->applyCenterImpulse(friction);
 160                                 }
 161                         }
 162
 163                         if (shapeProps->m_do_rot_fh) {
 164                                 const double *ogl_mat = cl_object->getMatrix();
 165                                 MT_Vector3 up(&ogl_mat[8]);
 166                                 MT_Vector3 t_spring = up.cross(normal) * matProps->m_fh_spring;
 167                                 MT_Vector3 ang_vel = cl_object->getAngularVelocity();
 168
 169                                 // only rotations that tilt relative to the normal are damped
 170                                 ang_vel -= ang_vel.dot(normal) * normal;
 171
 172                                 MT_Vector3 t_damp = ang_vel * matProps->m_fh_damping;
 173
 174                                 cl_object->addAngularVelocity(t_spring - t_damp);
 175                         }
 176                 }
 177         }
 178
 179         return DT_CONTINUE;
 180 }
 181
 182
 183