Exemple #1
0
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;
}
Exemple #2
0
 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;
 }
Exemple #3
0
/* ************************************************************************* */
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);
}
Exemple #4
0
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;
}
Exemple #5
0
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();
    }
}
Exemple #6
0
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;
}
Exemple #7
0
        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
Exemple #8
0
inline void  GraphicsSystem::MatrixRotate(float x, float y, float z, float a)
{
    MatrixStack[curMatrix] *= Quat(AxisAngle(x, y, z, a));
    ResetViewMatrix();
}
Exemple #9
0
//--------------------------------------------------------
//	
//--------------------------------------------------------
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;
}
Exemple #10
0
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);
}