btConeShape.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2009 Erwin Coumans  http://bulletphysics.org
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 #include "btConeShape.h"
00017 
00018 
00019 
00020 btConeShape::btConeShape (btScalar radius,btScalar height): btConvexInternalShape (),
00021 m_radius (radius),
00022 m_height(height)
00023 {
00024         m_shapeType = CONE_SHAPE_PROXYTYPE;
00025         setConeUpIndex(1);
00026         btVector3 halfExtents;
00027         m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height));
00028 }
00029 
00030 btConeShapeZ::btConeShapeZ (btScalar radius,btScalar height):
00031 btConeShape(radius,height)
00032 {
00033         setConeUpIndex(2);
00034 }
00035 
00036 btConeShapeX::btConeShapeX (btScalar radius,btScalar height):
00037 btConeShape(radius,height)
00038 {
00039         setConeUpIndex(0);
00040 }
00041 
00043 void    btConeShape::setConeUpIndex(int upIndex)
00044 {
00045         switch (upIndex)
00046         {
00047         case 0:
00048                         m_coneIndices[0] = 1;
00049                         m_coneIndices[1] = 0;
00050                         m_coneIndices[2] = 2;
00051                 break;
00052         case 1:
00053                         m_coneIndices[0] = 0;
00054                         m_coneIndices[1] = 1;
00055                         m_coneIndices[2] = 2;
00056                 break;
00057         case 2:
00058                         m_coneIndices[0] = 0;
00059                         m_coneIndices[1] = 2;
00060                         m_coneIndices[2] = 1;
00061                 break;
00062         default:
00063                 btAssert(0);
00064         };
00065 }
00066 
00067 btVector3 btConeShape::coneLocalSupport(const btVector3& v) const
00068 {
00069         
00070         btScalar halfHeight = m_height * btScalar(0.5);
00071 
00072  if (v[m_coneIndices[1]] > v.length() * m_sinAngle)
00073  {
00074         btVector3 tmp;
00075 
00076         tmp[m_coneIndices[0]] = btScalar(0.);
00077         tmp[m_coneIndices[1]] = halfHeight;
00078         tmp[m_coneIndices[2]] = btScalar(0.);
00079         return tmp;
00080  }
00081   else {
00082     btScalar s = btSqrt(v[m_coneIndices[0]] * v[m_coneIndices[0]] + v[m_coneIndices[2]] * v[m_coneIndices[2]]);
00083     if (s > SIMD_EPSILON) {
00084       btScalar d = m_radius / s;
00085           btVector3 tmp;
00086           tmp[m_coneIndices[0]] = v[m_coneIndices[0]] * d;
00087           tmp[m_coneIndices[1]] = -halfHeight;
00088           tmp[m_coneIndices[2]] = v[m_coneIndices[2]] * d;
00089           return tmp;
00090     }
00091     else  {
00092                 btVector3 tmp;
00093                 tmp[m_coneIndices[0]] = btScalar(0.);
00094                 tmp[m_coneIndices[1]] = -halfHeight;
00095                 tmp[m_coneIndices[2]] = btScalar(0.);
00096                 return tmp;
00097         }
00098   }
00099 
00100 }
00101 
00102 btVector3       btConeShape::localGetSupportingVertexWithoutMargin(const btVector3& vec) const
00103 {
00104                 return coneLocalSupport(vec);
00105 }
00106 
00107 void    btConeShape::batchedUnitVectorGetSupportingVertexWithoutMargin(const btVector3* vectors,btVector3* supportVerticesOut,int numVectors) const
00108 {
00109         for (int i=0;i<numVectors;i++)
00110         {
00111                 const btVector3& vec = vectors[i];
00112                 supportVerticesOut[i] = coneLocalSupport(vec);
00113         }
00114 }
00115 
00116 
00117 btVector3       btConeShape::localGetSupportingVertex(const btVector3& vec)  const
00118 {
00119         btVector3 supVertex = coneLocalSupport(vec);
00120         if ( getMargin()!=btScalar(0.) )
00121         {
00122                 btVector3 vecnorm = vec;
00123                 if (vecnorm .length2() < (SIMD_EPSILON*SIMD_EPSILON))
00124                 {
00125                         vecnorm.setValue(btScalar(-1.),btScalar(-1.),btScalar(-1.));
00126                 } 
00127                 vecnorm.normalize();
00128                 supVertex+= getMargin() * vecnorm;
00129         }
00130         return supVertex;
00131 }
00132 
00133 
00134 void    btConeShape::setLocalScaling(const btVector3& scaling)
00135 {
00136         int axis = m_coneIndices[1];
00137         int r1 = m_coneIndices[0];
00138         int r2 = m_coneIndices[2];
00139         m_height *= scaling[axis] / m_localScaling[axis];
00140         m_radius *= (scaling[r1] / m_localScaling[r1] + scaling[r2] / m_localScaling[r2]) / 2;
00141         m_sinAngle = (m_radius / btSqrt(m_radius * m_radius + m_height * m_height));
00142         btConvexInternalShape::setLocalScaling(scaling);
00143 }