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 {