public static void btTransformAabb( btIVector3 localAabbMin, btIVector3 localAabbMax, double margin, ref btTransform trans, out btVector3 aabbMinOut, out btVector3 aabbMaxOut )
{
Debug.Assert( localAabbMin.X <= localAabbMax.X );
Debug.Assert( localAabbMin.Y <= localAabbMax.Y );
Debug.Assert( localAabbMin.Z <= localAabbMax.Z );
btVector3 tmp;
localAabbMax.Sub( localAabbMin, out tmp );
btVector3 localHalfExtents; tmp.Mult( (double)( 0.5 ), out localHalfExtents );
btVector3 localCenter = localHalfExtents;
btVector3.Zero.Mult( margin, out tmp );
localHalfExtents.Add( ref tmp, out localHalfExtents );
//localHalfExtents += btVector3( margin, margin, margin );
btMatrix3x3 abs_b; trans.m_basis.absolute( out abs_b );
btVector3 center; trans.Apply( ref localCenter, out center );
btVector3 extent; localHalfExtents.dot3( ref abs_b.m_el0, ref abs_b.m_el1, ref abs_b.m_el2, out extent );
center.Sub( ref extent, out aabbMinOut );
center.Add( ref extent, out aabbMaxOut );
}
public void setOrigin( btIVector3 origin )
{
origin.Copy( out m_origin );
}
public void Apply( btIVector3 v, out btVector3 result )
{
result.x = m_el0.dot( v );
result.y = m_el1.dot( v );
result.z = m_el2.dot( v );
result.w = 0;
}
public void setInterpolate3( btIVector3 v0, btIVector3 v1, double rt )
{
double s = 1.0 - rt;
x = s * v0.X + rt * v1.X;
y = s * v0.Y + rt * v1.Y;
z = s * v0.Z + rt * v1.Z;
w = 0;
//don't do the unused w component
// m_co[3] = s v0[3] + rt v1[3];
}
/*@brief Return the transform of the vector */
#if InterfacesDidntSuck
public void Apply( btIVector3 x, out btVector3 result )
{
btVector3 tmp;
x.dot3( ref m_basis, out tmp );
tmp.Add( ref m_origin, out result );
}
public static void setInterpolate3( out btVector3 result, btIVector3 v0, btIVector3 v1, double rt )
{
double s = 1.0 - rt;
result.x = s * v0.X + rt * v1.X;
result.y = s * v0.Y + rt * v1.Y;
result.z = s * v0.Z + rt * v1.Z;
result.w = 0;
//don't do the unused w component
// m_co[3] = s v0[3] + rt v1[3];
}
public double dot( btIVector3 v )
{
return x * v.X +
y * v.Y +
z * v.Z;
}
public void Sub( btIVector3 b, out btVector3 result )
{
result.x = x - b.X;
result.y = y - b.Y;
result.z = z - b.Z;
result.w = w - b.W;
}
public void AddScale( btIVector3 b, double s, out btVector3 result )
{
result.x = x + b[0] * s;
result.y = y + b[1] * s;
result.z = z + b[2] * s;
result.w = w + b[3] * s;
}
public void Add( btIVector3 b, out btVector3 result )
{
result.x = x + b.X;
result.y = y + b.Y;
result.z = z + b.Z;
result.w = w + b.W;
}
