public btMatrix3x3( ref btVector3 v0, ref btVector3 v1, ref btVector3 v2 )
{
m_el0.x = v0.x; m_el0.y = v0.y; m_el0.z = v0.z; m_el0.w = 0;
m_el1.x = v1.x; m_el1.y = v1.y; m_el1.z = v1.z; m_el1.w = 0;
m_el2.x = v2.x; m_el2.y = v2.y; m_el2.z = v2.z; m_el2.w = 0;
m_el3.x = 0; m_el3.y = 0; m_el3.z = 0; m_el3.w = 1;
}
internal void GetVoxelCenterCoords( out btVector3 result )
{
result.x = X + 0.5f;
result.y = Y + 0.5f;
result.z = Z + 0.5f;
result.w = 0;
}
internal void SetPlayerPosition( VoxelWorld world, ref btVector3 v )
{
{
//Player_Position.x = v.x;
//Player_Position.y = v.y;
//Player_Position.z = v.z;
Player_Sector.x = (int)( v.x / ( world.VoxelBlockSize * VoxelSector.ZVOXELBLOCSIZE_X ) );
Player_Sector.y = (int)( v.y / ( world.VoxelBlockSize * VoxelSector.ZVOXELBLOCSIZE_Y ) );
Player_Sector.z = (int)( v.z / ( world.VoxelBlockSize * VoxelSector.ZVOXELBLOCSIZE_Z ) );
//Player_Voxel.x = (int)( v.x / world.VoxelBlockSize );
//Player_Voxel.y = (int)( v.y / world.VoxelBlockSize );
//Player_Voxel.z = (int)( v.z / world.VoxelBlockSize );
}
}
public btVector3( ref btVector3 v )
{
this.x = v.x; this.y = v.y; this.z = v.z; this.w = v.w;
}
public void ApplyInverse( ref btVector3 v, out btVector3 result )
{
result.x = tdotx( ref v );
result.y = tdoty( ref v );
result.z = tdotz( ref v );
result.w = 0;
}
public void Mult( ref btVector3 v, out btVector3 result )
{
result.x = m_el0.dot( ref v );
result.y = m_el1.dot( ref v );
result.z = m_el2.dot( ref v );
result.w = m_el3.dot( ref v );
}
/*
public btMatrix3x3 inverse()
{
btVector3 co = new btVector3( cofac( 1, 1, 2, 2 ), cofac( 1, 2, 2, 0 ), cofac( 1, 0, 2, 1 ) );
float det = m_el0.dot( ref co );
#if PARANOID_ASSERTS
Debug.Assert( det != 0 );
#endif
float s = 1.0 / det;
return new btMatrix3x3( co.x * s, cofac( 0, 2, 2, 1 ) * s, cofac( 0, 1, 1, 2 ) * s,
co.y * s, cofac( 0, 0, 2, 2 ) * s, cofac( 0, 2, 1, 0 ) * s,
co.z * s, cofac( 0, 1, 2, 0 ) * s, cofac( 0, 0, 1, 1 ) * s );
}
*/
public void inverse( out btMatrix3x3 result )
{
btVector3 co = new btVector3( cofac( 1, 1, 2, 2 ), cofac( 1, 2, 2, 0 ), cofac( 1, 0, 2, 1 ) );
float det = m_el0.dot( ref co );
#if PARANOID_ASSERTS
Debug.Assert( det != 0 );
#endif
float s = btScalar.BT_ONE / det;
result.m_el0.x = co.x * s;
result.m_el0.y = cofac( 0, 2, 2, 1 ) * s;
result.m_el0.z = cofac( 0, 1, 1, 2 ) * s;
result.m_el0.w = 0;
result.m_el1.x = co.y * s;
result.m_el1.y = cofac( 0, 0, 2, 2 ) * s;
result.m_el1.z = cofac( 0, 2, 1, 0 ) * s;
result.m_el1.w = 0;
result.m_el2.x = co.z * s;
result.m_el2.y = cofac( 0, 1, 2, 0 ) * s;
result.m_el2.z = cofac( 0, 0, 1, 1 ) * s;
result.m_el2.w = 0;
result.m_el3.x = 0;
result.m_el3.y = 0;
result.m_el3.z = 0;
result.m_el3.w = 0;
}
public float tdotx( ref btVector3 v )
{
return m_el0.x * v.x + m_el1.x * v.y + m_el2.x * v.z;
}
public float tdotz( ref btVector3 v )
{
return m_el0.z * v.x + m_el1.z * v.y + m_el2.z * v.z;
}
public btVector3 cross( btVector3 v )
{
return new btVector3( y * v.z - z * v.y,
z * v.x - x * v.z,
x * v.y - y * v.x );
}
/*@brief Create a scaled copy of the matrix
@param s Scaling vector The elements of the vector will scale each column */
public void scaled( ref btVector3 s, out btMatrix3x3 result )
{
btMatrix3x3.setValue( out result,
m_el0.x * s.x, m_el0.y * s.y, m_el0.z * s.z,
m_el1.x * s.x, m_el1.y * s.y, m_el1.z * s.z,
m_el2.x * s.x, m_el2.y * s.y, m_el2.z * s.z );
}
/*@brief Return a vector will the absolute values of each element */
public void absolute( out btVector3 result )
{
result.x = btScalar.btFabs( x ); result.y = btScalar.btFabs( y ); result.z = btScalar.btFabs( z ); result.w = 0;
}
public static btVector3 btAabbSupport( ref btVector3 halfExtents, ref btVector3 supportDir )
{
return new btVector3( ( supportDir.x < 0.0 ) ? (float)-halfExtents.x : (float)halfExtents.x,
( supportDir.y < 0.0 ) ? (float)-halfExtents.y : (float)halfExtents.y,
( supportDir.z < 0 ) ? (float)-halfExtents.z : (float)halfExtents.z );
}
public static void getCenter( ref btVector3 min, ref btVector3 max, out btVector3 half )
{
half.x = ( min.x + max.x ) * btScalar.BT_HALF;
half.y = ( min.y + max.y ) * btScalar.BT_HALF;
half.z = ( min.z + max.z ) * btScalar.BT_HALF;
half.w = ( min.w + max.w ) * btScalar.BT_HALF; ;
}
public static void getHalfExtent( ref btVector3 min, ref btVector3 max, out btVector3 half )
{
half.x = ( max.x - min.x ) * btScalar.BT_HALF;
half.y = ( max.y - min.y ) * btScalar.BT_HALF;
half.z = ( max.z - min.z ) * btScalar.BT_HALF;
half.w = ( max.w - min.w ) * btScalar.BT_HALF; ;
}
public static void btPlaneSpace1( ref btVector3 n, out btVector3 p, out btVector3 q )
{
if( btScalar.btFabs( n.z ) > btScalar.SIMDSQRT12 )
{
// choose p in y-z plane
float a = n.y * n.y + n.z * n.z;
float k = btScalar.btRecipSqrt( a );
p.x = 0;
p.y = -n[2] * k;
p.z = n[1] * k;
// set q = n x p
q.x = a * k;
q.y = -n.x * p.z;
q.z = n.x * p.y;
}
else
{
// choose p in x-y plane
float a = n.x * n.x + n.y * n.y;
float k = btScalar.btRecipSqrt( a );
p.x = -n.y * k;
p.y = n.x * k;
p.z = 0;
// set q = n x p
q.x = -n.z * p.x;
q.y = n.z * p.x;
q.z = a * k;
}
q.w = 0;
p.w = 0;
}
public void getColumn( int i, out btVector3 result )
{
switch( i )
{
default:
#if PARANOID_ASSERTS
Debug.Assert( i > 3 || i < 0 );
#endif
case 0:
result = new btVector3( m_el0.x, m_el1.x, m_el2.x );
break;
case 1:
result = new btVector3( m_el0.y, m_el1.y, m_el2.y );
break;
case 2:
result = new btVector3( m_el0.z, m_el1.z, m_el2.z );
break;
case 3:
result = new btVector3( m_el0.w, m_el1.w, m_el2.w );
break;
}
return;
}
public float dot( btVector3 v )
{
return x * v.x +
y * v.y +
z * v.z;
}
public void getRow( int i, out btVector3 result )
{
switch( i )
{
default:
#if PARANOID_ASSERTS
Debug.Assert( i > 3 || i < 0 );
#endif
case 0: result = m_el0; return;
case 1: result = m_el1; return;
case 2: result = m_el2; return;
case 3: result = m_el3; return;
}
}
/*@brief Add a vector to this one
@param The vector to add to this one */
public void Add( ref btVector3 b, out btVector3 result )
{
result.x = x + b.x;
result.y = y + b.y;
result.z = z + b.z;
result.w = w + b.w;
}
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases.
///Solve33 is from Box2d, thanks to Erin Catto,
public void solve33( ref btVector3 b, out btVector3 result )
{
btVector3 col1 = getColumn( 0 );
btVector3 col2 = getColumn( 1 );
btVector3 col3 = getColumn( 2 );
btVector3 tmp;
col2.cross( ref col3, out tmp );
float det = col1.dot( ref tmp );
if( btScalar.btFabs( det ) > btScalar.SIMD_EPSILON )
{
det = 1.0f / det;
}
col2.cross( ref col3, out tmp );
result.x = det * b.dot( ref tmp );
b.cross( ref col3, out tmp );
result.y = det * col1.dot( ref tmp );
col2.cross( ref b, out tmp );
result.z = det * col1.dot( ref tmp );
result.w = 0;
}
/*
///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
void btSwapScalarEndian( float sourceVal, float destVal)
{
# ifdef BT_USE_DOUBLE_PRECISION
byte dest = ( byte) destVal;
byte src = ( byte) &sourceVal;
dest = src[7];
dest[1] = src[6];
dest[2] = src[5];
dest[3] = src[4];
dest[4] = src[3];
dest[5] = src[2];
dest[6] = src[1];
dest[7] = src[0];
#else
byte dest = ( byte) destVal;
byte src = ( byte) &sourceVal;
dest = src[3];
dest[1] = src[2];
dest[2] = src[1];
dest[3] = src[0];
#endif //BT_USE_DOUBLE_PRECISION
}
///btSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
void btSwapVector3Endian( btVector3 sourceVec, ref btVector3 destVec)
{
for( int i = 0; i < 4; i++ )
{
btSwapScalarEndian( sourceVec[i], destVec[i] );
}
}
///btUnSwapVector3Endian swaps vector endianness, useful for network and cross-platform serialization
void btUnSwapVector3Endian( ref btVector3 vector)
{
btVector3 swappedVec;
for( int i = 0; i < 4; i++ )
{
btSwapScalarEndian( vector[i], swappedVec[i] );
}
vector = swappedVec;
}
*/
/*
void btVector3::serializeFloat(struct btVector3FloatData& dataOut)
{
///could also do a memcpy, check if it is worth it
for (int i = 0; i<4;i++)
dataOut.m_floats[i] = float(m_floats[i]);
}
void btVector3::deSerializeFloat(stringstruct btVector3FloatData& dataIn)
{
for (int i = 0; i<4;i++)
m_floats[i] = (float)( dataIn.m_floats[i]);
}
void btVector3::serializeDouble(struct btVector3DoubleData& dataOut)
{
///could also do a memcpy, check if it is worth it
for (int i = 0; i<4;i++)
dataOut.m_floats[i] = float(m_floats[i]);
}
void btVector3::deSerializeDouble(stringstruct btVector3DoubleData& dataIn)
{
for (int i = 0; i<4;i++)
m_floats[i] = (float)( dataIn.m_floats[i]);
}
void btVector3::serialize(struct btVector3Data& dataOut)
{
///could also do a memcpy, check if it is worth it
for (int i = 0; i<4;i++)
dataOut.m_floats[i] = m_floats[i];
}
void btVector3::deSerialize(stringstruct btVector3Data& dataIn)
{
for (int i = 0; i<4;i++)
m_floats[i] = dataIn.m_floats[i];
}
*/
public bool Equals( ref btVector3 q )
{
return ( q.x == x ) && ( q.y == y ) && ( q.z == z ) && ( q.w == w );
}
public float tdoty( ref btVector3 v )
{
return m_el0.y * v.x + m_el1.y * v.y + m_el2.y * v.z;
}
public virtual void SetPosition( btVector3 NewLocation )
{
ViewDirection.Translate( ref NewLocation );
}
public static void Mult( ref btMatrix3x3 m, ref btVector3 v, out btVector3 result )
{
result.x = m.m_el0.dot( ref v );
result.y = m.m_el1.dot( ref v );
result.z = m.m_el2.dot( ref v );
result.w = m.m_el3.dot( ref v );
}
/*@brief Return the linear interpolation between two vectors
@param v1 One vector
@param v2 The other vector
@param t The ration of this to v (t = 0 => return v1, t=1 => return v2) */
public static void lerp( btVector3 v1, btVector3 v2, float t, out btVector3 result )
{
v1.lerp( ref v2, t, out result );
}
public long maxDot( btVector3[] array, long array_count, out float dotOut )
{
{
float maxDot1 = -btScalar.SIMD_INFINITY;
int i = 0;
int ptIndex = -1;
for( i = 0; i < array_count; i++ )
{
float dot = array[i].dot( ref this );
if( dot > maxDot1 )
{
maxDot1 = dot;
ptIndex = i;
}
}
dotOut = maxDot1;
return ptIndex;
}
}
public void Apply( ref btVector3 v, out btVector3 result )
{
result.x = m_el0.dot( ref v );
result.y = m_el1.dot( ref v );
result.z = m_el2.dot( ref v );
result.w = 0;
}
public void ApplyRotation( ref btVector3 m, out btVector3 result )
{
#if COLUMN_MAJOR_EXPECTED
result.x = this.m_el0.x * m.x +
this.m_el0.y * m.y +
this.m_el0.z * m.z;
result.y = this.m_el1.x * m.x +
this.m_el1.y * m.y +
this.m_el1.z * m.z;
result.z = this.m_el2.x * m.x +
this.m_el2.y * m.y +
this.m_el2.z * m.z;
#else
result.x = this.m_el0.x * m.x +
this.m_el1.x * m.y +
this.m_el2.x * m.z;
result.y = this.m_el0.y * m.x +
this.m_el1.y * m.y +
this.m_el2.y * m.z;
result.z = this.m_el0.z * m.x +
this.m_el1.z * m.y +
this.m_el2.z * m.z;
#endif
result.w = 0;
}
public long minDot( btVector3[] array, long array_count, ref float dotOut )
{
{
float minDot = btScalar.SIMD_INFINITY;
int i = 0;
int ptIndex = -1;
for( i = 0; i < array_count; i++ )
{
float dot = array[i].dot( ref this );
if( dot < minDot )
{
minDot = dot;
ptIndex = i;
}
}
dotOut = minDot;
return ptIndex;
}
}
