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 }
  50
  51 DT_Bool SM_FhObject::ray_hit(void *client_data,
  52                                                   void *client_object1,
  53                                                   void *client_object2,
  54                                                   const DT_CollData *coll_data)
  55 {
  56
  57         SM_FhObject *fh_object  = dynamic_cast<SM_FhObject *>((SM_Object *)client_object2);
  58         if (!fh_object)
  59         {
  60                 std::swap(client_object1, client_object2);
  61                 fh_object  = dynamic_cast<SM_FhObject *>((SM_Object *)client_object2);
  62         }
  63
  64         SM_Object   *hit_object = (SM_Object *)client_object1;
  65         const SM_MaterialProps *matProps = hit_object->getMaterialProps();
  66
  67         if ((matProps == 0) || (matProps->m_fh_distance < MT_EPSILON)) {
  68                 return DT_CONTINUE;
  69         }
  70
  71         SM_Object           *cl_object  = fh_object->getParentObject();
  72
  73         assert(fh_object);
  74
  75         if (hit_object == cl_object) {
  76                 // Shot myself in the foot...
  77                 return DT_CONTINUE;
  78         }
  79
  80         const SM_ShapeProps *shapeProps = cl_object->getShapeProps();
  81
  82         // Exit if the client object is not dynamic.
  83         if (shapeProps == 0) {
  84                 return DT_CONTINUE;
  85         }
  86
  87         MT_Point3 lspot;
  88         MT_Vector3 normal;
  89
  90         DT_Vector3 from, to, dnormal;
  91         DT_Scalar dlspot;
  92         fh_object->getPosition().getValue(from);
  93         fh_object->getSpot().getValue(to);
  94
  95
  96         if (DT_ObjectRayCast(hit_object->getObjectHandle(),
  97                                                  from,
  98                                                  to,
  99                                                  1.,
 100                                                  &dlspot,
 101                                                  dnormal)) {
 102
 103                 lspot = fh_object->getPosition() + (fh_object->getSpot() - fh_object->getPosition()) * dlspot;
 104                 const MT_Vector3& ray_dir = fh_object->getRayDirection();
 105                 MT_Scalar dist = MT_distance(fh_object->getPosition(), lspot) -
 106                         cl_object->getMargin() - shapeProps->m_radius;
 107
 108                 normal = MT_Vector3(dnormal).safe_normalized();
 109
 110                 if (dist < matProps->m_fh_distance) {
 111
 112                         if (shapeProps->m_do_fh) {
 113                                 lspot -= hit_object->getPosition();
 114                                 MT_Vector3 rel_vel = cl_object->getLinearVelocity() - hit_object->getVelocity(lspot);
 115                                 MT_Scalar rel_vel_ray = ray_dir.dot(rel_vel);
 116                                 MT_Scalar spring_extent = 1.0 - dist / matProps->m_fh_distance;
 117
 118                                 MT_Scalar i_spring = spring_extent * matProps->m_fh_spring;
 119                                 MT_Scalar i_damp =   rel_vel_ray * matProps->m_fh_damping;
 120
 121                                 cl_object->addLinearVelocity(-(i_spring + i_damp) * ray_dir);
 122                                 if (matProps->m_fh_normal) {
 123                                         cl_object->addLinearVelocity(
 124                                                 (i_spring + i_damp) *
 125                                                 (normal - normal.dot(ray_dir) * ray_dir));
 126                                 }
 127
 128                                 MT_Vector3 lateral = rel_vel - rel_vel_ray * ray_dir;
 129                                 const SM_ShapeProps *shapeProps = cl_object->getShapeProps();
 130
 131                                 if (shapeProps->m_do_anisotropic) {
 132                                         MT_Matrix3x3 lcs(cl_object->getOrientation());
 133                                         MT_Vector3 loc_lateral = lateral * lcs;
 134                                         const MT_Vector3& friction_scaling =
 135                                                 shapeProps->m_friction_scaling;
 136
 137                                         loc_lateral.scale(friction_scaling[0],
 138                                                                           friction_scaling[1],
 139                                                                           friction_scaling[2]);
 140                                         lateral = lcs * loc_lateral;
 141                                 }
 142
 143
 144                                 MT_Scalar rel_vel_lateral = lateral.length();
 145
 146                                 if (rel_vel_lateral > MT_EPSILON) {
 147                                         MT_Scalar friction_factor = matProps->m_friction;
 148                                         MT_Scalar max_friction = friction_factor * MT_max(MT_Scalar(0.0), i_spring);
 149
 150                                         MT_Scalar rel_mom_lateral = rel_vel_lateral /
 151                                                 cl_object->getInvMass();
 152
 153                                         MT_Vector3 friction =
 154                                                 (rel_mom_lateral > max_friction) ?
 155                                                 -lateral * (max_friction / rel_vel_lateral) :
 156                                                 -lateral;
 157
 158                                         cl_object->applyCenterImpulse(friction);
 159                                 }
 160                         }
 161
 162                         if (shapeProps->m_do_rot_fh) {
 163                                 const double *ogl_mat = cl_object->getMatrix();
 164                                 MT_Vector3 up(&ogl_mat[8]);
 165                                 MT_Vector3 t_spring = up.cross(normal) * matProps->m_fh_spring;
 166                                 MT_Vector3 ang_vel = cl_object->getAngularVelocity();
 167
 168                                 // only rotations that tilt relative to the normal are damped
 169                                 ang_vel -= ang_vel.dot(normal) * normal;
 170
 171                                 MT_Vector3 t_damp = ang_vel * matProps->m_fh_damping;
 172
 173                                 cl_object->addAngularVelocity(t_spring - t_damp);
 174                         }
 175                 }
 176         }
 177
 178         return DT_CONTINUE;
 179 }
 180
 181
 182