Blender V2.61 - r43446

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 }