CIwFMat Physics::GetMatrixFromWheelInfo( const btWheelInfo w, const btRigidBody* b )
{
CIwFMat modelMatrix( CIwMat::g_Identity );
const btTransform btTrans = w.m_worldTransform;
const btVector3 pos = btTrans.getOrigin();
const float posX = pos.getX();
const float posY = pos.getY();
const float posZ = pos.getZ();
const CIwFVec3 position( posX, posY, posZ );
const btQuaternion q = btTrans.getRotation();
CIwFVec3 rot;
QuaternionToEuler( q, rot );
const btQuaternion qBody = b->getOrientation();
CIwFVec3 rotBody;
QuaternionToEuler( qBody, rotBody );
CIwFMat mat = CIwFMat::g_Identity;
mat.PostRotateX( rot.x );
mat.PostRotateY( -rotBody.y ); // mat.PostRotateY( rot.y ); fix for rotating wheels by y axis, there are glitches after 360 degrees rotating
mat.PostRotateZ( -rotBody.z );
modelMatrix.CopyRot( mat );
modelMatrix.SetTrans( position );
return modelMatrix;
}
CIwFVec3 Physics::GetRotationFromRigidBody( const btRigidBody* b )
{
CIwFVec3 rotation;
const btTransform btTrans = b->getWorldTransform();
const btQuaternion q = btTrans.getRotation();
QuaternionToEuler( q, rotation );
rotation *= static_cast<float>( BPU_180_PI );
return rotation;
}
void Physics::QuaternionToEuler( const CIwFQuat& TQuat, btVector3 &TEuler )
{
float a[3];
const float w = TQuat.s;
const float x = TQuat.x;
const float y = TQuat.y;
const float z = TQuat.z;
QuaternionToEuler( w, x, y, z, a );
TEuler.setX( a[0] );
TEuler.setY( a[1] );
TEuler.setZ( a[2] );
}
void Physics::QuaternionToEuler( const btQuaternion &TQuat, btVector3 &TEuler )
{
float a[3];
const float w = TQuat.getW();
const float x = TQuat.getX();
const float y = TQuat.getY();
const float z = TQuat.getZ();
QuaternionToEuler( w, x, y, z, a );
TEuler.setX( a[0] );
TEuler.setY( a[1] );
TEuler.setZ( a[2] );
}
//----------------------------
// ボーンの回転角度を制限する
//----------------------------
void cPMDIK::limitAngle( Vector4 *pvec4Out, const Vector4 *pvec4Src )
{
Vector3 vec3Angle;
// XYZ軸回転の取得
QuaternionToEuler( &vec3Angle, pvec4Src );
// 角度制限
if( vec3Angle.x < -3.14159f ) vec3Angle.x = -3.14159f;
if( -0.002f < vec3Angle.x ) vec3Angle.x = -0.002f;
vec3Angle.y = 0.0f;
vec3Angle.z = 0.0f;
// XYZ軸回転からクォータニオンへ
QuaternionCreateEuler( pvec4Out, &vec3Angle );
}
void Physics::QuaternionToEuler( const CIwFQuat& TQuat, CIwFVec3 &TEuler )
{
float a[3];
const float w = TQuat.s;
const float x = TQuat.x;
const float y = TQuat.y;
const float z = TQuat.z;
QuaternionToEuler( w, x, y, z, a );
// heading
TEuler.z = a[2];
// bank
TEuler.x = a[0];
// attitude
TEuler.y = a[1];
}
void Physics::QuaternionToEuler( const btQuaternion &TQuat, CIwFVec3 &TEuler )
{
float a[3];
const float w = TQuat.getW();
const float x = TQuat.getX();
const float y = TQuat.getY();
const float z = TQuat.getZ();
QuaternionToEuler( w, x, y, z, a );
// heading
TEuler.z = a[2];
// bank
TEuler.x = a[0];
// attitude
TEuler.y = a[1];
}
void simImu2krakenImu(const sensor_msgs::ImuConstPtr &msg){
kraken_msgs::imuData d;
d.data[kraken_sensors::accelX]=msg->linear_acceleration.x;
d.data[kraken_sensors::accelY]=msg->linear_acceleration.y;
d.data[kraken_sensors::accelZ]=msg->linear_acceleration.z;
geometry_msgs::Vector3 euler;
QuaternionToEuler(msg->orientation,euler);
d.data[kraken_sensors::roll]=euler.x;
d.data[kraken_sensors::pitch]=euler.y;
d.data[kraken_sensors::yaw]=euler.z;
d.data[kraken_sensors::gyroX]=msg->angular_velocity.x;
d.data[kraken_sensors::gyroY]=msg->angular_velocity.y;
d.data[kraken_sensors::gyroZ]=msg->angular_velocity.z;
_kraken_imu.publish(d);
}
// Runs the physics simulation.
// - TDeltaTime tells the simulation how much time has passed since the last frame so the simulation can run independently of the frame rate.
void CTBulletHelper::UpdatePhysics(u32 TDeltaTime) {
World->stepSimulation(TDeltaTime * 0.001f, 60);
btRigidBody *TObject;
// Relay the object's orientation to irrlicht
for(core::list<btRigidBody *>::Iterator it = Objects.begin(); it != Objects.end(); ++it) {
//UpdateRender(*Iterator);
scene::ISceneNode *Node = static_cast<scene::ISceneNode *>((*it)->getUserPointer());
TObject = *it;
// Set position
btVector3 Point = TObject->getCenterOfMassPosition();
Node->setPosition(core::vector3df((f32)Point[0], (f32)Point[1], (f32)Point[2]));
// Set rotation
btVector3 EulerRotation;
QuaternionToEuler(TObject->getOrientation(), EulerRotation);
Node->setRotation(core::vector3df(EulerRotation[0], EulerRotation[1], EulerRotation[2]));
}
}
vector3df QuaternionToEuler(const Quaternion& quat)
{
return QuaternionToEuler(ConvertQuaternion(quat));
}
void VenomModuleStart(GameMemory* memory) {
SystemInfo* sys = &memory->systemInfo;
GameData* data = PushStruct(GameData, &memory->mainBlock);
memory->userdata = data;
RenderState* rs = &memory->renderState;
BeginProfileEntry("Initalize Terrain Generator");
InitalizeTerrainGenerator(&data->terrain, V3(0.0, 0.0, 0.0), &memory->mainBlock);
EndProfileEntry();
rs->terrain = &data->terrain;
GetEngine()->physicsSimulation.terrain = &data->terrain;
#if 0
{
ModelData data = {};
data = ImportExternalModelData(VENOM_ASSET_FILE("axis.obj"), 0);
FILE* file = fopen("test.txt", "wb");
assert(file != 0);
for(size_t i = 0; i < data.meshData.vertexCount; i++) {
fprintf(file, "V3{%ff, %ff, %ff},\n", data.meshData.vertices[i].position.x,
data.meshData.vertices[i].position.y, data.meshData.vertices[i].position.z);
}
fprintf(file, "\n");
for(size_t i = 0; i < data.meshData.indexCount; i++) {
fprintf(file, "%u,", data.meshData.indices[i]);
if(i % 3 == 0) fprintf(file, "\n");
}
}
#endif
InitializeCamera(&data->camera, 45*DEG2RAD, 0.1f, 10000.0f, sys->screenWidth, sys->screenHeight);
data->camera.position = {4, 10, 2};
EntityContainerInit(&data->entityContainer, 1024, 8);
{
AssetManifest *assetManifest = &memory->assetManifest;
EntityContainer *entityContainer = &data->entityContainer;
EntityIndex player_index;
Entity *player = CreateEntity(EntityType_Player, &player_index, entityContainer);
assign_model_to_entity(player_index, GetModelID("player", assetManifest), assetManifest, entityContainer);
ModelAsset *asset = GetModelAsset(player->modelID, assetManifest);
player->animation_state.model_id = player->modelID;
player->position = player->position += V3(1, 5.0f, 1);
data->playerEntityIndex = player_index;
#if 0
Orientation cameraOrientation = CalculateCameraOrientationForTrackTarget(player->position);
data->camera.position = cameraOrientation.position;
V3 eulerRotation = QuaternionToEuler(cameraOrientation.rotation);
data->camera.pitch = eulerRotation.x;
data->camera.yaw = eulerRotation.y;
#endif
RNGSeed seed(15);
ScatterInRectangle(&seed, -128, -128, 256, 256, 8, 8, [&](V2 point) {
Entity *e = CreateEntity(EntityType_StaticObject, entityContainer);
e->modelID = GetModelID("Tree", assetManifest);
e->position.x = point.x;
e->position.z = point.y;
});
}
}
void VenomModuleRender(GameMemory* memory) {
RenderState* rs = &memory->renderState;
GameData* data = (GameData*)memory->userdata;
EditorData* editorData = &memory->editor;
EditorData* editor = editorData;
const V3 sunDirection = Normalize(V3(1.0, 0.5, 0.0f));
AddDirectionalLight(sunDirection, V3(1.0, 1.0, 1.0), &rs->drawList);
auto sunModelID = GetModelID("Sun", GetAssetManifest());
auto sunModel = GetModelAsset(sunModelID);
AddStaticModelToDrawList(&rs->drawList, sunModel, sunDirection * 10);
const F32 deltaTime = memory->deltaTime;
EntityContainer* entityContainer = &data->entityContainer;
auto engine = GetEngine();
EntityBlock* block = entityContainer->firstAvaibleBlock;
for (U64 i = 0; i < entityContainer->capacityPerBlock; i++) {
if (block->flags[i] & EntityFlag_PRESENT) {
Entity* entity = &block->entities[i];
if(block->types[i] == EntityType_PointLight) {
AddShadowCastingPointLight(entity->position, entity->pointLight.color,
entity->pointLight.radius, &rs->drawList);
}
ModelAsset *model = GetModelAsset(entity->modelID, &memory->assetManifest);
if (model == nullptr) continue;
bool is_entity_animated = model->jointCount > 0;
if (block->flags[i] & EntityFlag_VISIBLE) {
if (editor->selectedEntities.ContainsValue(i)) {
V3 rotation = QuaternionToEuler(entity->rotation);
AddOutlinedModelToDrawList(&rs->drawList, model, entity->position, rotation);
} else if (is_entity_animated) {
V3 rotation = QuaternionToEuler(entity->rotation);
V3 position = entity->position;
position.y += model->size.y * 0.5f;
AddAnimatedModelToDrawList(&rs->drawList, model, &entity->animation_state, position, rotation);
} else {
V3 rotation = QuaternionToEuler(entity->rotation);
V3 position = entity->position;
position.y += model->size.y * 0.5f;
AddStaticModelToDrawList(&rs->drawList, model, position, rotation);
}
}
}
}
Camera *camera = nullptr;
camera = &data->camera;
#if 0
if (editor->isEditorVisible) {
//camera = &editor->editorCamera;
..draw_debug_camera(&data->camera);
} else {
