public virtual void drawCapsule( double radius, double halfHeight, int upAxis, ref btTransform transform, ref btVector3 color )
{
int stepDegrees = 30;
btVector3 capStart = btVector3.Zero;
capStart[upAxis] = -halfHeight;
btVector3 capEnd = btVector3.Zero;
capEnd[upAxis] = halfHeight;
// Draw the ends
{
btTransform childTransform = transform;
transform.Apply( ref capStart, out childTransform.m_origin );
{
btVector3 up = childTransform.m_basis.getColumn( ( upAxis + 1 ) % 3 );
btVector3 axis = childTransform.m_basis.getColumn( upAxis );
axis.Invert( out axis );
double minTh = -btScalar.SIMD_HALF_PI;
double maxTh = btScalar.SIMD_HALF_PI;
double minPs = -btScalar.SIMD_HALF_PI;
double maxPs = btScalar.SIMD_HALF_PI;
drawSpherePatch( ref childTransform.m_origin, ref up, ref axis, radius, minTh, maxTh, minPs, maxPs, ref color, (double)stepDegrees, false );
}
}
{
btTransform childTransform = transform;
transform.Apply( ref capEnd, out childTransform.m_origin );
{
btVector3 up = childTransform.m_basis.getColumn( ( upAxis + 1 ) % 3 );
btVector3 axis = childTransform.m_basis.getColumn( upAxis );
double minTh = -btScalar.SIMD_HALF_PI;
double maxTh = btScalar.SIMD_HALF_PI;
double minPs = -btScalar.SIMD_HALF_PI;
double maxPs = btScalar.SIMD_HALF_PI;
drawSpherePatch( ref childTransform.m_origin, ref up, ref axis, radius, minTh, maxTh, minPs, maxPs, ref color, (double)stepDegrees, false );
}
}
// Draw some additional lines
//btVector3 start = transform.getOrigin();
for( int i = 0; i < 360; i += stepDegrees )
{
capEnd[( upAxis + 1 ) % 3] = capStart[( upAxis + 1 ) % 3] = btScalar.btSin( (double)i * btScalar.SIMD_RADS_PER_DEG ) * radius;
capEnd[( upAxis + 2 ) % 3] = capStart[( upAxis + 2 ) % 3] = btScalar.btCos( (double)i * btScalar.SIMD_RADS_PER_DEG ) * radius;
btVector3 a, b;
transform.m_basis.Apply( ref capStart, out a );
a.Add( ref transform.m_origin, out a );
transform.m_basis.Apply( ref capEnd, out b );
a.Add( ref transform.m_origin, out b );
drawLine( ref a, ref b, ref color );
}
}
public virtual void drawPlane( ref btVector3 planeNormal, double planeConst, ref btTransform transform, ref btVector3 color )
{
btVector3 planeOrigin; planeNormal.Mult( planeConst, out planeOrigin );
btVector3 vec0, vec1;
btVector3.btPlaneSpace1( ref planeNormal, out vec0, out vec1 );
double vecLen = 100;
vec0.Mult( vecLen, out vec0 );
vec1.Mult( vecLen, out vec1 );
btVector3 pt0; planeOrigin.Add( ref vec0, out pt0 );
btVector3 pt1; planeOrigin.Sub( ref vec0, out pt1 );
btVector3 pt2; planeOrigin.Add( ref vec1, out pt2 );
btVector3 pt3; planeOrigin.Sub( ref vec1, out pt3 );
transform.Apply( ref pt0, out pt0 );
transform.Apply( ref pt1, out pt1 );
transform.Apply( ref pt2, out pt2 );
transform.Apply( ref pt3, out pt3 );
drawLine( ref pt0, ref pt1, ref color );
drawLine( ref pt2, ref pt3, ref color );
}
public void putLine( ref btTransform trans, double a, double b, double c, double d, double e, double f, ref btVector3 color )
{
btVector3 av;
btVector3 bv;
btVector3.setValue( out av, a, b, c );
btVector3.setValue( out bv, d, e, f );
trans.Apply( ref av, out av ); trans.Apply( ref bv, out bv );
drawLine( ref av, ref bv, ref color );
}
public virtual void drawTransform( ref btTransform transform, double orthoLen )
{
//btIVector3 start = transform.getOrigin();
btVector3 a;
btVector3 b;
btVector3.setValue( out b, orthoLen, 0, 0 );
transform.Apply( ref b, out a );
b.setValue( 0.7, 0, 0 );
drawLine( ref transform.m_origin, ref a, ref b );
btVector3.setValue( out b, 0, orthoLen, 0 );
transform.Apply( ref b, out a );
b.setValue( 0, 0.7, 0 );
drawLine( ref transform.m_origin, ref a, ref b );
btVector3.setValue( out b, 0, 0, orthoLen );
transform.Apply( ref b, out a );
b.setValue( 0, 0, 0.7 );
drawLine( ref transform.m_origin, ref a, ref b );
}
///synchronizes world transform from physics to user
///Bullet only calls the update of worldtransform for active objects
public virtual void setWorldTransform( ref btTransform centerOfMassWorldTrans )
{
centerOfMassWorldTrans.Apply( ref m_centerOfMassOffset, out m_graphicsWorldTrans );
}
/*@brief Return the transform of the vector */
public static void Apply( ref btTransform t, ref btVector3 x, out btVector3 result )
{
t.Apply( ref x, out result );
}
/*@brief Set the current transform as the value of the product of two transforms
@param t1 Transform 1
@param t2 Transform 2
This = Transform1 Transform2 */
public void mult( ref btTransform t1, ref btTransform t2 )
{
t1.m_basis.Apply( ref t2.m_basis, out m_basis );
t1.Apply( ref t2.m_origin, out m_origin );
}