Quat& Quat::SetLookDirection(const Vect &dir)
{
Vect newDir = -dir.GetNorm();
Quat XZrot, YZrot;
XZrot.SetIdentity();
YZrot.SetIdentity();
BOOL bXZValid = !CloseFloat(newDir.x, 0.0f, EPSILON) || !CloseFloat(newDir.z, 0.0f, EPSILON);
BOOL bYZValid = !CloseFloat(newDir.y, 0.0f, EPSILON);
if(bXZValid)
XZrot = AxisAngle(0.0f, 1.0f, 0.0f, atan2f(newDir.x, newDir.z));
if(bYZValid)
YZrot = AxisAngle(-1.0f, 0.0f, 0.0f, asinf(newDir.y));
if(!bXZValid)
*this = YZrot;
else if(!bYZValid)
*this = XZrot;
else
*this = XZrot *= YZrot;
return *this;
}
void IMU::calibrateGyro()
{
gyroBias = AxisAngle();
for(uint16_t i=0; i<IMU_GYRO_CALIBRATION_LENGTH; i++)
{
MPU6050::refresh();
gyroBias += AxisAngle(IMU_GYRO_X, IMU_GYRO_Y, IMU_GYRO_Z);
}
gyroBias /= IMU_GYRO_CALIBRATION_LENGTH;
}
/* ************************************************************************* */
Rot3 Rot3::Random(boost::mt19937& rng) {
// TODO allow any engine without including all of boost :-(
Unit3 axis = Unit3::Random(rng);
boost::uniform_real<double> randomAngle(-M_PI, M_PI);
double angle = randomAngle(rng);
return AxisAngle(axis, angle);
}
void MeshEntity::SetMesh(CTSTR lpMesh)
{
traceIn(MeshEntity::SetMesh);
if(!mesh)
{
mesh = RM->GetMesh(lpMesh);
if(!MaterialList.Num())
{
MaterialList.SetSize(mesh->DefaultMaterialList.Num());
for(int i=0; i<MaterialList.Num(); i++)
{
String resName = mesh->DefaultMaterialList[i].name; //converting from utf to wide
MaterialList[i] = GetMaterial(String(resName));
}
}
}
if(bCastCompositeShadow && !bStaticGeometry && level->UsesLightmaps())
{
compositeShadow = CreateFrameBuffer(64, 64, GS_RGBA, FALSE);
shadowDecal = CreateObjectParam(ShadowDecal, this);
shadowDecal->texture = compositeShadow;
shadowDecal->bRenderable = FALSE;
shadowDecal->SetPos(mesh->bounds.GetCenter());
shadowDecal->shadowRot = AxisAngle(1.0f, 0.0f, 0.0f, RAD(-80.0f));
shadowDecal->shadowRot *= AxisAngle(0.0f, 1.0f, 0.0f, RAD(-10.0f));
shadowDecal->decalColor.Set(1.0f, 1.0f, 1.0f, 0.75f);
shadowDecal->Attach(this);
}
VertBuffer = mesh->VertBuffer;
VBData *vbd = VertBuffer->GetData();
VertList = vbd->VertList.Array();
FaceNormalList = NULL;
bounds = mesh->bounds;
traceOut;
}
Quat& Quat::MakeFromDirection(const Vect &dir)
{
Vect identDir(0.0f, 0.0f, 1.0f);
if(dir.CloseTo(identDir))
return SetIdentity();
else
{
float cosValue = fabsf(dir.z); //dir.Dot(identDir);
float angle = acos(cosValue);
Vect cross = Vect(dir.y, -dir.x, 0.0f).Norm(); //(dir.GetCross(identDir).Norm()
return MakeFromAxisAngle(AxisAngle(cross.x, cross.y, cross.z, angle)).Norm();
}
}
uint8_t IMU::refresh()
{
// Calculate loop time
loopStopTime = micros();
loopTime = (loopStopTime - loopStartTime) / 1000000.0;
loopStartTime = loopStopTime;
// Refresh gyroscope and accelerometer
if(!MPU6050::refresh()) return 0;
// Calculate gyroscope rotation
gyro = AxisAngle(IMU_GYRO_X, IMU_GYRO_Y, IMU_GYRO_Z);
#ifdef IMU_GYRO_CALIBRATION
gyro -= gyroBias;
#endif
gyro *= loopTime;
// Correct accelerometer error
#ifdef IMU_ACCEL_ENABLE
Vector up = Vector(0.0, 0.0, -1.0).rotate(attitude.conjugate());
accel = Vector(IMU_ACCEL_X, IMU_ACCEL_Y, IMU_ACCEL_Z);
AxisAngle accelError = AxisAngle::fromTwoVectors(up, accel);
gyro -= (accelError * IMU_ACCEL_COEFF);
#endif
// Correct magnetometer error
#ifdef IMU_MAGNET_ENABLE
if(HMC5883L::refresh())
{
#ifdef IMU_MAGNET_CALIBRATION
float xTemp = IMU_MAGNET_X - MAGNET_HARD_X;
float yTemp = IMU_MAGNET_Y - MAGNET_HARD_Y;
float zTemp = IMU_MAGNET_Z - MAGNET_HARD_Z;
magnet = Vector(
MAGNET_SOFT_XX * xTemp + MAGNET_SOFT_XY * yTemp + MAGNET_SOFT_XZ * zTemp,
MAGNET_SOFT_YX * xTemp + MAGNET_SOFT_YY * yTemp + MAGNET_SOFT_YZ * zTemp,
MAGNET_SOFT_ZX * xTemp + MAGNET_SOFT_ZY * yTemp + MAGNET_SOFT_ZZ * zTemp
);
#else
magnet = Vector(IMU_MAGNET_X, IMU_MAGNET_Y, IMU_MAGNET_Z);
#endif
magnet = magnet.reject(up);
Vector north = Vector(1.0, 0.0, 0.0).rotate(attitude.conjugate());
AxisAngle magnetError = AxisAngle::fromTwoVectors(north, magnet);
gyro -= (magnetError * IMU_MAGNET_COEFF);
}
#endif
// Update attitude
Quaternion loopRotation = Quaternion::fromAxisAngle(gyro);
attitude = attitude * loopRotation;
attitude.normalise();
// Update angles
#ifdef IMU_ANGLES
roll = attitude.getRoll();
pitch = attitude.getPitch();
yaw = attitude.getYaw();
rollRate = loopRotation.getRoll() / loopTime;
pitchRate = loopRotation.getPitch() / loopTime;
yawRate = loopRotation.getYaw() / loopTime;
#endif
return 1;
}
Serial.print(IMU::getYawRate());
Serial.println();
#endif
}
}
#endif
// ================================ Private ================================ //
float IMU::loopTime = 0.0;
uint32_t IMU::loopStartTime = 0;
uint32_t IMU::loopStopTime = 0;
Quaternion IMU::attitude = Quaternion();
AxisAngle IMU::gyro = AxisAngle();
#ifdef IMU_ACCEL_ENABLE
Vector IMU::accel = Vector();
#endif
#ifdef IMU_MAGNET_ENABLE
Vector IMU::magnet = Vector();
#endif
#ifdef IMU_ANGLES
float IMU::roll = 0.0;
float IMU::pitch = 0.0;
float IMU::yaw = 0.0;
float IMU::rollRate = 0.0;
float IMU::pitchRate = 0.0;
float IMU::yawRate = 0.0;
#endif
inline void GraphicsSystem::MatrixRotate(float x, float y, float z, float a)
{
MatrixStack[curMatrix] *= Quat(AxisAngle(x, y, z, a));
ResetViewMatrix();
}
//--------------------------------------------------------
//
//--------------------------------------------------------
SyncBool TLFile::ImportBinaryData(const TString& DataString,TBinary& BinaryData,TRef DataType)
{
/*
// work out the type of data
TRefRef BinaryDataType = BinaryData.GetDataTypeHint();
// check for conflicting type hints
if ( DataType.IsValid() && BinaryDataType.IsValid() && DataType != BinaryDataType )
{
TDebugString Debug_String;
Debug_String << "Data import type hint mismatch; Tried to import as " << DataType << " but binary says it's " << BinaryDataType;
TLDebug_Break( Debug_String );
// fall through to use the data type embedded in the binary data
DataType = BinaryDataType;
}
else if ( BinaryDataType.IsValid() && !DataType.IsValid() )
{
// use the type specified in the binary
DataType = BinaryDataType;
}
*/
// import the data based on the type
u32 CharIndex = 0;
switch ( DataType.GetData() )
{
case TLBinary_TypeRef(float):
{
float f;
if ( !TLString::ReadNextFloatArray( DataString, CharIndex, &f, 1 ) )
return SyncFalse;
BinaryData.Write( f );
return SyncTrue;
}
case TLBinary_TypeRef(float2):
{
float2 f;
if ( !TLString::ReadNextFloatArray( DataString, CharIndex, f.GetData(), f.GetSize() ) )
return SyncFalse;
BinaryData.Write( f );
return SyncTrue;
}
case TLBinary_TypeRef(float3):
{
float3 f;
if ( !TLString::ReadNextFloatArray( DataString, CharIndex, f.GetData(), f.GetSize() ) )
return SyncFalse;
BinaryData.Write( f );
return SyncTrue;
}
case TLBinary_TypeRef(float4):
{
float4 f;
if ( !TLString::ReadNextFloatArray( DataString, CharIndex, f.GetData(), f.GetSize() ) )
return SyncFalse;
BinaryData.Write( f );
return SyncTrue;
}
case TLBinary_TypeRef(TQuaternion):
{
float4 f;
if ( !TLString::ReadNextFloatArray( DataString, CharIndex, f.GetData(), f.GetSize() ) )
return SyncFalse;
// convert to normalised quaternion
TLMaths::TQuaternion Quat( f );
Quat.Normalise();
BinaryData.Write( Quat );
return SyncTrue;
}
case TLBinary_TypeRef(TEuler):
{
float3 f;
if ( !TLString::ReadNextFloatArray( DataString, CharIndex, f.GetData(), f.GetSize() ) )
return SyncFalse;
// convert to Euler type
TLMaths::TEuler Euler( f );
BinaryData.Write( Euler );
return SyncTrue;
}
case TLBinary_TypeRef(TAxisAngle):
{
float4 f;
if ( !TLString::ReadNextFloatArray( DataString, CharIndex, f.GetData(), f.GetSize() ) )
return SyncFalse;
// convert to normalised quaternion
TLMaths::TAxisAngle AxisAngle( f );
BinaryData.Write( AxisAngle );
return SyncTrue;
}
case TLBinary_TypeRef(TRef):
{
TRef Ref( DataString );
BinaryData.Write( Ref );
return SyncTrue;
}
case TLBinary_TypeRef_String:
{
// do string cleanup, convert "\n" to a linefeed etc
if ( TLString::IsStringDirty( DataString ) )
{
TString OutputString = DataString;
TLString::CleanString( OutputString );
BinaryData.WriteString( OutputString );
}
else
{
// already clean, just write the original
BinaryData.WriteString( DataString );
}
return SyncTrue;
}
case TLBinary_TypeRef(TColour):
{
float4 f;
if ( !TLString::ReadNextFloatArray( DataString, CharIndex, f.GetData(), f.GetSize() ) )
return SyncFalse;
// check range
// gr: use TLDebug_CheckInRange() ?
if ( f.x > 1.0f || f.x < 0.0f ||
f.y > 1.0f || f.y < 0.0f ||
f.z > 1.0f || f.z < 0.0f ||
f.w > 1.0f || f.w < 0.0f )
{
if ( !TLDebug_Break( TString("Colour float type has components out of range (0..1); %.3f,%.3f,%.3f,%.3f", f.x, f.y, f.z, f.w) ) )
return SyncFalse;
}
TColour Colour( f );
BinaryData.Write( Colour );
return SyncTrue;
}
case TLBinary_TypeRef(TColour24):
{
Type3<s32> Colours;
if ( !TLString::ReadNextInteger( DataString, CharIndex, Colours.x ) ) return SyncFalse;
if ( !TLString::ReadNextInteger( DataString, CharIndex, Colours.y ) ) return SyncFalse;
if ( !TLString::ReadNextInteger( DataString, CharIndex, Colours.z ) ) return SyncFalse;
// check range
// gr: use TLDebug_CheckInRange() ?
if ( Colours.x > 255 || Colours.x < 0 ||
Colours.y > 255 || Colours.y < 0 ||
Colours.z > 255 || Colours.z < 0 )
{
if ( !TLDebug_Break( TString("Colour24 type has components out of range (0..255); %d,%d,%d", Colours.x, Colours.y, Colours.z ) ) )
return SyncFalse;
}
TColour24 Colour( Colours.x, Colours.y, Colours.z );
BinaryData.Write( Colour );
return SyncTrue;
}
case TLBinary_TypeRef(TColour32):
{
Type4<s32> Colours;
if ( !TLString::ReadNextInteger( DataString, CharIndex, Colours.x ) ) return SyncFalse;
if ( !TLString::ReadNextInteger( DataString, CharIndex, Colours.y ) ) return SyncFalse;
if ( !TLString::ReadNextInteger( DataString, CharIndex, Colours.z ) ) return SyncFalse;
if ( !TLString::ReadNextInteger( DataString, CharIndex, Colours.w ) ) return SyncFalse;
// check range
// gr: use TLDebug_CheckInRange() ?
if ( Colours.x > 255 || Colours.x < 0 ||
Colours.y > 255 || Colours.y < 0 ||
Colours.z > 255 || Colours.z < 0 ||
Colours.w > 255 || Colours.w < 0 )
{
if ( !TLDebug_Break( TString("Colour32 type has components out of range (0..255); %d,%d,%d,%d", Colours.x, Colours.y, Colours.z, Colours.w ) ) )
return SyncFalse;
}
TColour32 Colour( Colours.x, Colours.y, Colours.z, Colours.w );
BinaryData.Write( Colour );
return SyncTrue;
}
case TLBinary_TypeRef(TColour64):
{
Type4<s32> Colours;
if ( !TLString::ReadNextInteger( DataString, CharIndex, Colours.x ) ) return SyncFalse;
if ( !TLString::ReadNextInteger( DataString, CharIndex, Colours.y ) ) return SyncFalse;
if ( !TLString::ReadNextInteger( DataString, CharIndex, Colours.z ) ) return SyncFalse;
if ( !TLString::ReadNextInteger( DataString, CharIndex, Colours.w ) ) return SyncFalse;
// check range
// gr: use TLDebug_CheckInRange() ?
if ( Colours.x > 65535 || Colours.x < 0 ||
Colours.y > 65535 || Colours.y < 0 ||
Colours.z > 65535 || Colours.z < 0 ||
Colours.w > 65535 || Colours.w < 0 )
{
if ( !TLDebug_Break( TString("Colour64 type has components out of range (0..65535); %d,%d,%d,%d", Colours.x, Colours.y, Colours.z, Colours.w ) ) )
return SyncFalse;
}
TColour64 Colour( Colours.x, Colours.y, Colours.z, Colours.w );
BinaryData.Write( Colour );
return SyncTrue;
}
case TLBinary_TypeRef(u8):
return ImportBinaryDataIntegerInRange<u8>( BinaryData, DataString );
case TLBinary_TypeRef(s8):
return ImportBinaryDataIntegerInRange<s8>( BinaryData, DataString );
case TLBinary_TypeRef(u16):
return ImportBinaryDataIntegerInRange<u16>( BinaryData, DataString );
case TLBinary_TypeRef(s16):
return ImportBinaryDataIntegerInRange<s16>( BinaryData, DataString );
case TLBinary_TypeRef(u32):
return ImportBinaryDataIntegerInRange<u32>( BinaryData, DataString );
case TLBinary_TypeRef(s32):
return ImportBinaryDataIntegerInRange<s32>( BinaryData, DataString );
case TLBinary_TypeRef(Bool):
{
// read first char, we can work out true/false/0/1 from that
if ( DataString.GetLength() == 0 )
return SyncFalse;
const TChar& BoolChar = DataString.GetCharAt(0);
if ( BoolChar == 't' || BoolChar == 'T' || BoolChar == '1' )
{
BinaryData.Write( (Bool)TRUE );
return SyncTrue;
}
else if ( BoolChar == 'f' || BoolChar == 'F' || BoolChar == '0' )
{
BinaryData.Write( (Bool)FALSE );
return SyncTrue;
}
else
{
TLDebug_Break("Bool data is not True,False,0 or 1");
return SyncFalse;
}
}
default:
break;
};
TDebugString Debug_String;
Debug_String << "Unsupported/todo data type " << DataType << ". Data string: [" << DataString << "]";
TLDebug_Break( Debug_String );
return SyncFalse;
}
void D3D10System::DrawSpriteExRotate(Texture *texture, DWORD color, float x, float y, float x2, float y2, float degrees, float u, float v, float u2, float v2, float texDegrees)
{
if(!curPixelShader)
return;
if(!texture)
{
AppWarning(TEXT("Trying to draw a sprite with a NULL texture"));
return;
}
HANDLE hColor = curPixelShader->GetParameterByName(TEXT("outputColor"));
if(hColor)
curPixelShader->SetColor(hColor, color);
//------------------------------
// crop positional values
Vect2 totalSize = Vect2(x2-x, y2-y);
Vect2 invMult = Vect2(totalSize.x < 0.0f ? -1.0f : 1.0f, totalSize.y < 0.0f ? -1.0f : 1.0f);
totalSize.Abs();
if(y2-y < 0) {
float tempFloat = curCropping[1];
curCropping[1] = curCropping[3];
curCropping[3] = tempFloat;
}
if(x2-x < 0) {
float tempFloat = curCropping[0];
curCropping[0] = curCropping[2];
curCropping[2] = tempFloat;
}
x += curCropping[0] * invMult.x;
y += curCropping[1] * invMult.y;
x2 -= curCropping[2] * invMult.x;
y2 -= curCropping[3] * invMult.y;
//------------------------------
// crop texture coordinate values
float cropMult[4];
cropMult[0] = curCropping[0]/totalSize.x;
cropMult[1] = curCropping[1]/totalSize.y;
cropMult[2] = curCropping[2]/totalSize.x;
cropMult[3] = curCropping[3]/totalSize.y;
Vect2 totalUVSize = Vect2(u2-u, v2-v);
u += cropMult[0] * totalUVSize.x;
v += cropMult[1] * totalUVSize.y;
u2 -= cropMult[2] * totalUVSize.x;
v2 -= cropMult[3] * totalUVSize.y;
//------------------------------
// draw
VBData *data = spriteVertexBuffer->GetData();
data->VertList[0].Set(x, y, 0.0f);
data->VertList[1].Set(x, y2, 0.0f);
data->VertList[2].Set(x2, y, 0.0f);
data->VertList[3].Set(x2, y2, 0.0f);
if (!CloseFloat(degrees, 0.0f)) {
List<Vect> &coords = data->VertList;
Vect2 center(x+totalSize.x/2, y+totalSize.y/2);
Matrix rotMatrix;
rotMatrix.SetIdentity();
rotMatrix.Rotate(AxisAngle(0.0f, 0.0f, 1.0f, RAD(degrees)));
for (int i = 0; i < 4; i++) {
Vect val = coords[i]-Vect(center);
val.TransformVector(rotMatrix);
coords[i] = val;
coords[i] += Vect(center);
}
}
List<UVCoord> &coords = data->UVList[0];
coords[0].Set(u, v);
coords[1].Set(u, v2);
coords[2].Set(u2, v);
coords[3].Set(u2, v2);
if (!CloseFloat(texDegrees, 0.0f)) {
Matrix rotMatrix;
rotMatrix.SetIdentity();
rotMatrix.Rotate(AxisAngle(0.0f, 0.0f, 1.0f, -RAD(texDegrees)));
Vect2 minVal = Vect2(0.0f, 0.0f);
for (int i = 0; i < 4; i++) {
Vect val = Vect(coords[i]);
val.TransformVector(rotMatrix);
coords[i] = val;
minVal.ClampMax(coords[i]);
}
for (int i = 0; i < 4; i++)
coords[i] -= minVal;
}
spriteVertexBuffer->FlushBuffers();
LoadVertexBuffer(spriteVertexBuffer);
LoadTexture(texture);
Draw(GS_TRIANGLESTRIP);
}
