void DemoApplication::updateCamera() { /* printf("ele=%f, azi=%f, cp=[%f,%f,%f], ct=[%f,%f,%f], cu=[%f,%f,%f]\n", m_ele, m_azi, m_cameraPosition[0], m_cameraPosition[1], m_cameraPosition[2], m_cameraTargetPosition[0], m_cameraTargetPosition[1], m_cameraTargetPosition[2], m_cameraUp[0], m_cameraUp[1], m_cameraUp[2]); */ glMatrixMode(GL_PROJECTION); glLoadIdentity(); btScalar rele = m_ele * btScalar(0.01745329251994329547);// rads per deg btScalar razi = m_azi * btScalar(0.01745329251994329547);// rads per deg btQuaternion rot(m_cameraUp,razi); btVector3 eyePos(0,0,0); eyePos[m_forwardAxis] = -m_cameraDistance; btVector3 forward(eyePos[0],eyePos[1],eyePos[2]); if (forward.length2() < SIMD_EPSILON) { forward.setValue(1.f,0.f,0.f); } btVector3 right = m_cameraUp.cross(forward); btQuaternion roll(right,-rele); eyePos = btMatrix3x3(rot) * btMatrix3x3(roll) * eyePos; m_cameraPosition[0] = eyePos.getX(); m_cameraPosition[1] = eyePos.getY(); m_cameraPosition[2] = eyePos.getZ(); m_cameraPosition += m_cameraTargetPosition; if (m_glutScreenWidth == 0 && m_glutScreenHeight == 0) return; btScalar aspect; btVector3 extents; aspect = m_glutScreenWidth / (btScalar)m_glutScreenHeight; extents.setValue(aspect * 1.0f, 1.0f,0); if (m_ortho) { // reset matrix glLoadIdentity(); extents *= m_cameraDistance; btVector3 lower = m_cameraTargetPosition - extents; btVector3 upper = m_cameraTargetPosition + extents; //gluOrtho2D(lower.x, upper.x, lower.y, upper.y); glOrtho(lower.getX(), upper.getX(), lower.getY(), upper.getY(),-1000,1000); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); //glTranslatef(100,210,0); } else { // glFrustum (-aspect, aspect, -1.0, 1.0, 1.0, 10000.0); glFrustum (-aspect * m_frustumZNear, aspect * m_frustumZNear, -m_frustumZNear, m_frustumZNear, m_frustumZNear, m_frustumZFar); glMatrixMode(GL_MODELVIEW); glLoadIdentity(); gluLookAt(m_cameraPosition[0], m_cameraPosition[1], m_cameraPosition[2], m_cameraTargetPosition[0], m_cameraTargetPosition[1], m_cameraTargetPosition[2], m_cameraUp.getX(),m_cameraUp.getY(),m_cameraUp.getZ()); } }
Eigen::Rotation2D<double> Rototranslation2D::getRotation() const { Eigen::Rotation2D<double> rot(0); rot = rot.fromRotationMatrix(this->linear()); return rot; }
void Light::SetupShadowViews(Camera* mainCamera, Vector<AutoPtr<ShadowView> >& shadowViews, size_t& useIndex) { size_t numViews = NumShadowViews(); if (!numViews) return; if (shadowViews.Size() < useIndex + numViews) shadowViews.Resize(useIndex + numViews); int numVerticalSplits = (lightType == LIGHT_POINT || (lightType == LIGHT_DIRECTIONAL && NumShadowSplits() > 2)) ? 2 : 1; int actualShadowMapSize = shadowRect.Height() / numVerticalSplits; for (size_t i = 0; i < numViews; ++i) { if (!shadowViews[useIndex + i]) shadowViews[useIndex + i] = new ShadowView(); ShadowView* view = shadowViews[useIndex + i].Get(); view->Clear(); view->light = this; Camera& shadowCamera = view->shadowCamera; switch (lightType) { case LIGHT_DIRECTIONAL: { IntVector2 topLeft(shadowRect.left, shadowRect.top); if (i & 1) topLeft.x += actualShadowMapSize; if (i & 2) topLeft.y += actualShadowMapSize; view->viewport = IntRect(topLeft.x, topLeft.y, topLeft.x + actualShadowMapSize, topLeft.y + actualShadowMapSize); float splitStart = Max(mainCamera->NearClip(), (i == 0) ? 0.0f : ShadowSplit(i - 1)); float splitEnd = Min(mainCamera->FarClip(), ShadowSplit(i)); float extrusionDistance = mainCamera->FarClip(); // Calculate initial position & rotation shadowCamera.SetTransform(mainCamera->WorldPosition() - extrusionDistance * WorldDirection(), WorldRotation()); // Calculate main camera shadowed frustum in light's view space Frustum splitFrustum = mainCamera->WorldSplitFrustum(splitStart, splitEnd); const Matrix3x4& lightView = shadowCamera.ViewMatrix(); Frustum lightViewFrustum = splitFrustum.Transformed(lightView); // Fit the frustum inside a bounding box BoundingBox shadowBox; shadowBox.Define(lightViewFrustum); // If shadow camera is far away from the frustum, can bring it closer for better depth precision /// \todo The minimum distance is somewhat arbitrary float minDistance = mainCamera->FarClip() * 0.25f; if (shadowBox.min.z > minDistance) { float move = shadowBox.min.z - minDistance; shadowCamera.Translate(Vector3(0.0f, 0.f, move)); shadowBox.min.z -= move, shadowBox.max.z -= move; } shadowCamera.SetOrthographic(true); shadowCamera.SetFarClip(shadowBox.max.z); Vector3 center = shadowBox.Center(); Vector3 size = shadowBox.Size(); shadowCamera.SetOrthoSize(Vector2(size.x, size.y)); shadowCamera.SetZoom(1.0f); // Center shadow camera to the view space bounding box Vector3 pos(shadowCamera.WorldPosition()); Quaternion rot(shadowCamera.WorldRotation()); Vector3 adjust(center.x, center.y, 0.0f); shadowCamera.Translate(rot * adjust, TS_WORLD); // Snap to whole texels { Vector3 viewPos(rot.Inverse() * shadowCamera.WorldPosition()); float invSize = 1.0f / actualShadowMapSize; Vector2 texelSize(size.x * invSize, size.y * invSize); Vector3 snap(-fmodf(viewPos.x, texelSize.x), -fmodf(viewPos.y, texelSize.y), 0.0f); shadowCamera.Translate(rot * snap, TS_WORLD); } } break; case LIGHT_POINT: { static const Quaternion pointLightFaceRotations[] = { Quaternion(0.0f, 90.0f, 0.0f), Quaternion(0.0f, -90.0f, 0.0f), Quaternion(-90.0f, 0.0f, 0.0f), Quaternion(90.0f, 0.0f, 0.0f), Quaternion(0.0f, 0.0f, 0.0f), Quaternion(0.0f, 180.0f, 0.0f) }; IntVector2 topLeft(shadowRect.left, shadowRect.top); if (i & 1) topLeft.y += actualShadowMapSize; topLeft.x += ((unsigned)i >> 1) * actualShadowMapSize; view->viewport = IntRect(topLeft.x, topLeft.y, topLeft.x + actualShadowMapSize, topLeft.y + actualShadowMapSize); shadowCamera.SetTransform(WorldPosition(), pointLightFaceRotations[i]); shadowCamera.SetFov(90.0f); // Adjust zoom to avoid edge sampling artifacts (there is a matching adjustment in the shadow sampling) shadowCamera.SetZoom(0.99f); shadowCamera.SetFarClip(Range()); shadowCamera.SetNearClip(Range() * 0.01f); shadowCamera.SetOrthographic(false); shadowCamera.SetAspectRatio(1.0f); } break; case LIGHT_SPOT: view->viewport = shadowRect; shadowCamera.SetTransform(WorldPosition(), WorldRotation()); shadowCamera.SetFov(fov); shadowCamera.SetZoom(1.0f); shadowCamera.SetFarClip(Range()); shadowCamera.SetNearClip(Range() * 0.01f); shadowCamera.SetOrthographic(false); shadowCamera.SetAspectRatio(1.0f); break; } } // Setup shadow matrices now as camera positions have been finalized if (lightType != LIGHT_POINT) { shadowMatrices.Resize(numViews); for (size_t i = 0; i < numViews; ++i) { ShadowView* view = shadowViews[useIndex + i].Get(); Camera& shadowCamera = view->shadowCamera; float width = (float)shadowMap->Width(); float height = (float)shadowMap->Height(); Vector3 offset((float)view->viewport.left / width, (float)view->viewport.top / height, 0.0f); Vector3 scale(0.5f * (float)view->viewport.Width() / width, 0.5f * (float)view->viewport.Height() / height, 1.0f); offset.x += scale.x; offset.y += scale.y; scale.y = -scale.y; // OpenGL has different depth range #ifdef TURSO3D_OPENGL offset.z = 0.5f; scale.z = 0.5f; #endif Matrix4 texAdjust(Matrix4::IDENTITY); texAdjust.SetTranslation(offset); texAdjust.SetScale(scale); shadowMatrices[i] = texAdjust * shadowCamera.ProjectionMatrix() * shadowCamera.ViewMatrix(); } } else { // Point lights use an extra constant instead shadowMatrices.Clear(); Vector2 textureSize((float)shadowMap->Width(), (float)shadowMap->Height()); pointShadowParameters = Vector4(actualShadowMapSize / textureSize.x, actualShadowMapSize / textureSize.y, (float)shadowRect.left / textureSize.x, (float)shadowRect.top / textureSize.y); } // Calculate shadow mapping constants Camera& shadowCamera = shadowViews[useIndex]->shadowCamera; float nearClip = shadowCamera.NearClip(); float farClip = shadowCamera.FarClip(); float q = farClip / (farClip - nearClip); float r = -q * nearClip; shadowParameters = Vector4(0.5f / (float)shadowMap->Width(), 0.5f / (float)shadowMap->Height(), q, r); useIndex += numViews; }
void DemoCameraListener::PreUpdate (const NewtonWorld* const world, dFloat timestep) { // update the camera; DemoEntityManager* const scene = (DemoEntityManager*) NewtonWorldGetUserData(world); NewtonDemos* const mainWin = scene->GetRootWindow(); dMatrix targetMatrix (m_camera->GetNextMatrix()); int mouseX; int mouseY; mainWin->GetMousePosition (mouseX, mouseY); // slow down the Camera if we have a Body dFloat slowDownFactor = mainWin->IsShiftKeyDown() ? 0.5f/10.0f : 0.5f; // do camera translation if (mainWin->GetKeyState ('W')) { targetMatrix.m_posit += targetMatrix.m_front.Scale(m_frontSpeed * timestep * slowDownFactor); } if (mainWin->GetKeyState ('S')) { targetMatrix.m_posit -= targetMatrix.m_front.Scale(m_frontSpeed * timestep * slowDownFactor); } if (mainWin->GetKeyState ('A')) { targetMatrix.m_posit -= targetMatrix.m_right.Scale(m_sidewaysSpeed * timestep * slowDownFactor); } if (mainWin->GetKeyState ('D')) { targetMatrix.m_posit += targetMatrix.m_right.Scale(m_sidewaysSpeed * timestep * slowDownFactor); } if (mainWin->GetKeyState ('Q')) { targetMatrix.m_posit -= targetMatrix.m_up.Scale(m_sidewaysSpeed * timestep * slowDownFactor); } if (mainWin->GetKeyState ('E')) { targetMatrix.m_posit += targetMatrix.m_up.Scale(m_sidewaysSpeed * timestep * slowDownFactor); } // do camera rotation, only if we do not have anything picked bool buttonState = m_mouseLockState || mainWin->GetMouseKeyState(0); if (!m_targetPicked && buttonState) { int mouseSpeedX = mouseX - m_mousePosX; int mouseSpeedY = mouseY - m_mousePosY; if (mouseSpeedX > 0) { m_yaw = dMod(m_yaw + m_yawRate, 2.0f * 3.1416f); } else if (mouseSpeedX < 0){ m_yaw = dMod(m_yaw - m_yawRate, 2.0f * 3.1416f); } if (mouseSpeedY > 0) { m_pitch += m_pitchRate; } else if (mouseSpeedY < 0){ m_pitch -= m_pitchRate; } m_pitch = dClamp(m_pitch, dFloat (-80.0f * 3.1416f / 180.0f), dFloat (80.0f * 3.1416f / 180.0f)); } m_mousePosX = mouseX; m_mousePosY = mouseY; dMatrix matrix (dRollMatrix(m_pitch) * dYawMatrix(m_yaw)); dQuaternion rot (matrix); m_camera->SetMatrix (*scene, rot, targetMatrix.m_posit); UpdatePickBody(scene, timestep); }
Rototranslation2D Rototranslation2D::inverse() const { Eigen::Rotation2D<double> rot(-this->getAngle()); Eigen::Vector2d tNew(rot * this->getTranslation()); tNew = -tNew; return Rototranslation2D(rot, tNew); }
void LoadSkeleton(const ea::string& skeletonFileName) { // Process skeleton first (if found) XMLElement skeletonRoot; File skeletonFileSource(context_); skeletonFileSource.Open(skeletonFileName); if (!skelFile_->Load(skeletonFileSource)) PrintLine("Failed to load skeleton " + skeletonFileName); skeletonRoot = skelFile_->GetRoot(); if (skeletonRoot) { XMLElement bonesRoot = skeletonRoot.GetChild("bones"); XMLElement bone = bonesRoot.GetChild("bone"); while (bone) { unsigned index = bone.GetInt("id"); ea::string name = bone.GetAttribute("name"); if (index >= bones_.size()) bones_.resize(index + 1); // Convert from right- to left-handed XMLElement position = bone.GetChild("position"); float x = position.GetFloat("x"); float y = position.GetFloat("y"); float z = position.GetFloat("z"); Vector3 pos(x, y, -z); XMLElement rotation = bone.GetChild("rotation"); XMLElement axis = rotation.GetChild("axis"); float angle = -rotation.GetFloat("angle") * M_RADTODEG; x = axis.GetFloat("x"); y = axis.GetFloat("y"); z = axis.GetFloat("z"); Vector3 axisVec(x, y, -z); Quaternion rot(angle, axisVec); bones_[index].name_ = name; bones_[index].parentIndex_ = index; // Fill in the correct parent later bones_[index].bindPosition_ = pos; bones_[index].bindRotation_ = rot; bones_[index].bindScale_ = Vector3::ONE; bones_[index].collisionMask_ = 0; bones_[index].radius_ = 0.0f; bone = bone.GetNext("bone"); } // Go through the bone hierarchy XMLElement boneHierarchy = skeletonRoot.GetChild("bonehierarchy"); XMLElement boneParent = boneHierarchy.GetChild("boneparent"); while (boneParent) { ea::string bone = boneParent.GetAttribute("bone"); ea::string parent = boneParent.GetAttribute("parent"); unsigned i = 0, j = 0; for (i = 0; i < bones_.size() && bones_[i].name_ != bone; ++i); for (j = 0; j < bones_.size() && bones_[j].name_ != parent; ++j); if (i >= bones_.size() || j >= bones_.size()) ErrorExit("Found indeterminate parent bone assignment"); bones_[i].parentIndex_ = j; boneParent = boneParent.GetNext("boneparent"); } // Calculate bone derived positions for (unsigned i = 0; i < bones_.size(); ++i) { Vector3 derivedPosition = bones_[i].bindPosition_; Quaternion derivedRotation = bones_[i].bindRotation_; Vector3 derivedScale = bones_[i].bindScale_; unsigned index = bones_[i].parentIndex_; if (index != i) { for (;;) { derivedPosition = bones_[index].bindPosition_ + (bones_[index].bindRotation_ * (bones_[index].bindScale_ * derivedPosition)); derivedRotation = bones_[index].bindRotation_ * derivedRotation; derivedScale = bones_[index].bindScale_ * derivedScale; if (bones_[index].parentIndex_ != index) index = bones_[index].parentIndex_; else break; } } bones_[i].derivedPosition_ = derivedPosition; bones_[i].derivedRotation_ = derivedRotation; bones_[i].derivedScale_ = derivedScale; bones_[i].worldTransform_ = Matrix3x4(derivedPosition, derivedRotation, derivedScale); bones_[i].inverseWorldTransform_ = bones_[i].worldTransform_.Inverse(); } PrintLine("Processed skeleton"); } }
void WriteOutput(const ea::string& outputFileName, bool exportAnimations, bool rotationsOnly, bool saveMaterialList) { /// \todo Use save functions of Model & Animation classes // Begin serialization { File dest(context_); if (!dest.Open(outputFileName, FILE_WRITE)) ErrorExit("Could not open output file " + outputFileName); // ID dest.WriteFileID("UMD2"); // Vertexbuffers dest.WriteUInt(vertexBuffers_.size()); for (unsigned i = 0; i < vertexBuffers_.size(); ++i) vertexBuffers_[i].WriteData(dest); // Indexbuffers dest.WriteUInt(indexBuffers_.size()); for (unsigned i = 0; i < indexBuffers_.size(); ++i) indexBuffers_[i].WriteData(dest); // Subgeometries dest.WriteUInt(subGeometries_.size()); for (unsigned i = 0; i < subGeometries_.size(); ++i) { // Write bone mapping info from the first LOD level. It does not change for further LODs dest.WriteUInt(subGeometries_[i][0].boneMapping_.size()); for (unsigned k = 0; k < subGeometries_[i][0].boneMapping_.size(); ++k) dest.WriteUInt(subGeometries_[i][0].boneMapping_[k]); // Lod levels for this subgeometry dest.WriteUInt(subGeometries_[i].size()); for (unsigned j = 0; j < subGeometries_[i].size(); ++j) { dest.WriteFloat(subGeometries_[i][j].distance_); dest.WriteUInt((unsigned)subGeometries_[i][j].primitiveType_); dest.WriteUInt(subGeometries_[i][j].vertexBuffer_); dest.WriteUInt(subGeometries_[i][j].indexBuffer_); dest.WriteUInt(subGeometries_[i][j].indexStart_); dest.WriteUInt(subGeometries_[i][j].indexCount_); } } // Morphs dest.WriteUInt(morphs_.size()); for (unsigned i = 0; i < morphs_.size(); ++i) morphs_[i].WriteData(dest); // Skeleton dest.WriteUInt(bones_.size()); for (unsigned i = 0; i < bones_.size(); ++i) { dest.WriteString(bones_[i].name_); dest.WriteUInt(bones_[i].parentIndex_); dest.WriteVector3(bones_[i].bindPosition_); dest.WriteQuaternion(bones_[i].bindRotation_); dest.WriteVector3(bones_[i].bindScale_); Matrix3x4 offsetMatrix(bones_[i].derivedPosition_, bones_[i].derivedRotation_, bones_[i].derivedScale_); offsetMatrix = offsetMatrix.Inverse(); dest.Write(offsetMatrix.Data(), sizeof(Matrix3x4)); dest.WriteUByte(bones_[i].collisionMask_); if (bones_[i].collisionMask_ & 1u) dest.WriteFloat(bones_[i].radius_); if (bones_[i].collisionMask_ & 2u) dest.WriteBoundingBox(bones_[i].boundingBox_); } // Bounding box dest.WriteBoundingBox(boundingBox_); // Geometry centers for (unsigned i = 0; i < subGeometryCenters_.size(); ++i) dest.WriteVector3(subGeometryCenters_[i]); } if (saveMaterialList) { ea::string materialListName = ReplaceExtension(outputFileName, ".txt"); File listFile(context_); if (listFile.Open(materialListName, FILE_WRITE)) { for (unsigned i = 0; i < materialNames_.size(); ++i) { // Assume the materials will be located inside the standard Materials subdirectory listFile.WriteLine("Materials/" + ReplaceExtension(SanitateAssetName(materialNames_[i]), ".xml")); } } else PrintLine("Warning: could not write material list file " + materialListName); } XMLElement skeletonRoot = skelFile_->GetRoot("skeleton"); if (skeletonRoot && exportAnimations) { // Go through animations XMLElement animationsRoot = skeletonRoot.GetChild("animations"); if (animationsRoot) { XMLElement animation = animationsRoot.GetChild("animation"); while (animation) { ModelAnimation newAnimation; newAnimation.name_ = animation.GetAttribute("name"); newAnimation.length_ = animation.GetFloat("length"); XMLElement tracksRoot = animation.GetChild("tracks"); XMLElement track = tracksRoot.GetChild("track"); while (track) { ea::string trackName = track.GetAttribute("bone"); ModelBone* bone = nullptr; for (unsigned i = 0; i < bones_.size(); ++i) { if (bones_[i].name_ == trackName) { bone = &bones_[i]; break; } } if (!bone) ErrorExit("Found animation track for unknown bone " + trackName); AnimationTrack newAnimationTrack; newAnimationTrack.name_ = trackName; if (!rotationsOnly) newAnimationTrack.channelMask_ = CHANNEL_POSITION | CHANNEL_ROTATION; else newAnimationTrack.channelMask_ = CHANNEL_ROTATION; XMLElement keyFramesRoot = track.GetChild("keyframes"); XMLElement keyFrame = keyFramesRoot.GetChild("keyframe"); while (keyFrame) { AnimationKeyFrame newKeyFrame; // Convert from right- to left-handed XMLElement position = keyFrame.GetChild("translate"); float x = position.GetFloat("x"); float y = position.GetFloat("y"); float z = position.GetFloat("z"); Vector3 pos(x, y, -z); XMLElement rotation = keyFrame.GetChild("rotate"); XMLElement axis = rotation.GetChild("axis"); float angle = -rotation.GetFloat("angle") * M_RADTODEG; x = axis.GetFloat("x"); y = axis.GetFloat("y"); z = axis.GetFloat("z"); Vector3 axisVec(x, y, -z); Quaternion rot(angle, axisVec); // Transform from bind-pose relative into absolute pos = bone->bindPosition_ + pos; rot = bone->bindRotation_ * rot; newKeyFrame.time_ = keyFrame.GetFloat("time"); newKeyFrame.position_ = pos; newKeyFrame.rotation_ = rot; newAnimationTrack.keyFrames_.push_back(newKeyFrame); keyFrame = keyFrame.GetNext("keyframe"); } // Make sure keyframes are sorted from beginning to end ea::quick_sort(newAnimationTrack.keyFrames_.begin(), newAnimationTrack.keyFrames_.end(), CompareKeyFrames); // Do not add tracks with no keyframes if (newAnimationTrack.keyFrames_.size()) newAnimation.tracks_.push_back(newAnimationTrack); track = track.GetNext("track"); } // Write each animation into a separate file ea::string animationFileName = outputFileName.replaced(".mdl", ""); animationFileName += "_" + newAnimation.name_ + ".ani"; File dest(context_); if (!dest.Open(animationFileName, FILE_WRITE)) ErrorExit("Could not open output file " + animationFileName); dest.WriteFileID("UANI"); dest.WriteString(newAnimation.name_); dest.WriteFloat(newAnimation.length_); dest.WriteUInt(newAnimation.tracks_.size()); for (unsigned i = 0; i < newAnimation.tracks_.size(); ++i) { AnimationTrack& track = newAnimation.tracks_[i]; dest.WriteString(track.name_); dest.WriteUByte(track.channelMask_); dest.WriteUInt(track.keyFrames_.size()); for (unsigned j = 0; j < track.keyFrames_.size(); ++j) { AnimationKeyFrame& keyFrame = track.keyFrames_[j]; dest.WriteFloat(keyFrame.time_); if (track.channelMask_ & CHANNEL_POSITION) dest.WriteVector3(keyFrame.position_); if (track.channelMask_ & CHANNEL_ROTATION) dest.WriteQuaternion(keyFrame.rotation_); if (track.channelMask_ & CHANNEL_SCALE) dest.WriteVector3(keyFrame.scale_); } } animation = animation.GetNext("animation"); PrintLine("Processed animation " + newAnimation.name_); } } } }
void dgCollisionChamferCylinder::DebugCollision(const dgMatrix& matrixPtr, OnDebugCollisionMeshCallback callback, void* const userData) const { dgInt32 i; dgInt32 j; dgInt32 index; dgInt32 index0; dgInt32 brakes; dgInt32 slices; dgFloat32 sliceStep; dgFloat32 sliceAngle; dgFloat32 breakStep; dgFloat32 breakAngle; slices = 12; brakes = 24; sliceAngle = dgFloat32(0.0f); breakAngle = dgFloat32(0.0f); sliceStep = dgPI / slices; breakStep = dgPI2 / brakes; dgTriplex pool[24 * (12 + 1)]; dgMatrix rot(dgPitchMatrix(breakStep)); index = 0; for (j = 0; j <= slices; j++) { dgVector p0(-m_height * dgCos(sliceAngle), dgFloat32(0.0f), m_radius + m_height * dgSin(sliceAngle), dgFloat32(0.0f)); sliceAngle += sliceStep; for (i = 0; i < brakes; i++) { pool[index].m_x = p0.m_x; pool[index].m_y = p0.m_y; pool[index].m_z = p0.m_z; index++; p0 = rot.UnrotateVector(p0); } } // const dgMatrix &matrix = myBody.GetCollisionMatrix(); dgMatrix matrix(GetOffsetMatrix() * matrixPtr); matrix.TransformTriplex(&pool[0].m_x, sizeof(dgTriplex), &pool[0].m_x, sizeof(dgTriplex), 24 * (12 + 1)); dgTriplex face[32]; index = 0; for (j = 0; j < slices; j++) { index0 = index + brakes - 1; for (i = 0; i < brakes; i++) { face[0] = pool[index]; face[1] = pool[index0]; face[2] = pool[index0 + brakes]; face[3] = pool[index + brakes]; index0 = index; index++; callback(userData, 4, &face[0].m_x, 0); } } for (i = 0; i < brakes; i++) { face[i] = pool[i]; } callback(userData, 24, &face[0].m_x, 0); for (i = 0; i < brakes; i++) { face[i] = pool[brakes * (slices + 1) - i - 1]; } callback(userData, 24, &face[0].m_x, 0); }
void dgCollisionChamferCylinder::Init(dgFloat32 radius, dgFloat32 height) { // dgInt32 i; // dgInt32 j; // dgInt32 index; // dgInt32 index0; // dgFloat32 sliceStep; // dgFloat32 sliceAngle; // dgFloat32 breakStep; // dgFloat32 breakAngle; // dgEdge *edge; m_rtti |= dgCollisionChamferCylinder_RTTI; m_radius = dgAbsf(radius); m_height = dgAbsf(height * dgFloat32(0.5f)); m_radius = GetMax(dgFloat32(0.001f), m_radius - m_height); m_silhuette[0] = dgVector(m_height, m_radius, dgFloat32(0.0f), dgFloat32(0.0f)); m_silhuette[1] = dgVector(m_height, -m_radius, dgFloat32(0.0f), dgFloat32(0.0f)); m_silhuette[2] = dgVector(-m_height, -m_radius, dgFloat32(0.0f), dgFloat32(0.0f)); m_silhuette[3] = dgVector(-m_height, m_radius, dgFloat32(0.0f), dgFloat32(0.0f)); // m_tethaStep = GetDiscretedAngleStep (m_radius); // m_tethaStepInv = dgFloat32 (1.0f) / m_tethaStep; // m_delCosTetha = dgCos (m_tethaStep); // m_delSinTetha = dgSin (m_tethaStep); dgFloat32 sliceAngle = dgFloat32(0.0f); //dgFloat32 breakAngle = dgFloat32 (0.0f); dgFloat32 sliceStep = dgPI / DG_CHAMFERCYLINDER_SLICES; dgFloat32 breakStep = dgPI2 / DG_CHAMFERCYLINDER_BRAKES; dgMatrix rot(dgPitchMatrix(breakStep)); dgInt32 index = 0; for (dgInt32 j = 0; j <= DG_CHAMFERCYLINDER_SLICES; j++) { dgVector p0(-m_height * dgCos(sliceAngle), dgFloat32(0.0f), m_radius + m_height * dgSin(sliceAngle), dgFloat32(1.0f)); sliceAngle += sliceStep; for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i++) { m_vertex[index] = p0; index++; p0 = rot.UnrotateVector(p0); } } m_edgeCount = (4 * DG_CHAMFERCYLINDER_SLICES + 2) * DG_CHAMFERCYLINDER_BRAKES; m_vertexCount = DG_CHAMFERCYLINDER_BRAKES * (DG_CHAMFERCYLINDER_SLICES + 1); dgCollisionConvex::m_vertex = m_vertex; if (!m_shapeRefCount) { dgPolyhedra polyhedra(m_allocator); dgInt32 wireframe[DG_CHAMFERCYLINDER_SLICES + 10]; for (dgInt32 i = 0; i < DG_MAX_CHAMFERCYLINDER_DIR_COUNT; i++) { dgMatrix matrix( dgPitchMatrix( dgFloat32(dgPI2 * i) / DG_MAX_CHAMFERCYLINDER_DIR_COUNT)); m_shapesDirs[i] = matrix.RotateVector( dgVector(dgFloat32(0.0f), dgFloat32(1.0f), dgFloat32(0.0f), dgFloat32(0.0f))); } dgInt32 index = 0; for (dgInt32 j = 0; j < DG_CHAMFERCYLINDER_SLICES; j++) { dgInt32 index0 = index + DG_CHAMFERCYLINDER_BRAKES - 1; for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i++) { wireframe[0] = index; wireframe[1] = index0; wireframe[2] = index0 + DG_CHAMFERCYLINDER_BRAKES; wireframe[3] = index + DG_CHAMFERCYLINDER_BRAKES; index0 = index; index++; polyhedra.AddFace(4, wireframe); } } for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i++) { wireframe[i] = i; } polyhedra.AddFace(DG_CHAMFERCYLINDER_BRAKES, wireframe); for (dgInt32 i = 0; i < DG_CHAMFERCYLINDER_BRAKES; i++) { wireframe[i] = DG_CHAMFERCYLINDER_BRAKES * (DG_CHAMFERCYLINDER_SLICES + 1) - i - 1; } polyhedra.AddFace(DG_CHAMFERCYLINDER_BRAKES, wireframe); polyhedra.EndFace(); _ASSERTE(SanityCheck (polyhedra)); dgUnsigned64 i = 0; dgPolyhedra::Iterator iter(polyhedra); for (iter.Begin(); iter; iter++) { dgEdge* const edge = &(*iter); edge->m_userData = i; i++; } for (iter.Begin(); iter; iter++) { dgEdge* const edge = &(*iter); dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData]; ptr->m_vertex = edge->m_incidentVertex; ptr->m_next = &m_edgeArray[edge->m_next->m_userData]; ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData]; ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData]; } } m_shapeRefCount++; dgCollisionConvex::m_simplex = m_edgeArray; SetVolumeAndCG(); // CalculateDistanceTravel(); }
void AnimClip::copyFromNetworkAnim() { assert(_networkAnim && _networkAnim->isLoaded() && _skeleton); _anim.clear(); // build a mapping from animation joint indices to skeleton joint indices. // by matching joints with the same name. const FBXGeometry& geom = _networkAnim->getGeometry(); AnimSkeleton animSkeleton(geom); const auto animJointCount = animSkeleton.getNumJoints(); const auto skeletonJointCount = _skeleton->getNumJoints(); std::vector<int> jointMap; jointMap.reserve(animJointCount); for (int i = 0; i < animJointCount; i++) { int skeletonJoint = _skeleton->nameToJointIndex(animSkeleton.getJointName(i)); if (skeletonJoint == -1) { qCWarning(animation) << "animation contains joint =" << animSkeleton.getJointName(i) << " which is not in the skeleton, url =" << _url; } jointMap.push_back(skeletonJoint); } const int frameCount = geom.animationFrames.size(); _anim.resize(frameCount); for (int frame = 0; frame < frameCount; frame++) { const FBXAnimationFrame& fbxAnimFrame = geom.animationFrames[frame]; // init all joints in animation to default pose // this will give us a resonable result for bones in the model skeleton but not in the animation. _anim[frame].reserve(skeletonJointCount); for (int skeletonJoint = 0; skeletonJoint < skeletonJointCount; skeletonJoint++) { _anim[frame].push_back(_skeleton->getRelativeDefaultPose(skeletonJoint)); } for (int animJoint = 0; animJoint < animJointCount; animJoint++) { int skeletonJoint = jointMap[animJoint]; const glm::vec3& fbxAnimTrans = fbxAnimFrame.translations[animJoint]; const glm::quat& fbxAnimRot = fbxAnimFrame.rotations[animJoint]; // skip joints that are in the animation but not in the skeleton. if (skeletonJoint >= 0 && skeletonJoint < skeletonJointCount) { AnimPose preRot, postRot; if (usePreAndPostPoseFromAnim) { preRot = animSkeleton.getPreRotationPose(animJoint); postRot = animSkeleton.getPostRotationPose(animJoint); } else { // In order to support Blender, which does not have preRotation FBX support, we use the models defaultPose as the reference frame for the animations. preRot = AnimPose(glm::vec3(1.0f), _skeleton->getRelativeBindPose(skeletonJoint).rot, glm::vec3()); postRot = AnimPose::identity; } // cancel out scale preRot.scale = glm::vec3(1.0f); postRot.scale = glm::vec3(1.0f); AnimPose rot(glm::vec3(1.0f), fbxAnimRot, glm::vec3()); // adjust translation offsets, so large translation animatons on the reference skeleton // will be adjusted when played on a skeleton with short limbs. const glm::vec3& fbxZeroTrans = geom.animationFrames[0].translations[animJoint]; const AnimPose& relDefaultPose = _skeleton->getRelativeDefaultPose(skeletonJoint); float boneLengthScale = 1.0f; const float EPSILON = 0.0001f; if (fabsf(glm::length(fbxZeroTrans)) > EPSILON) { boneLengthScale = glm::length(relDefaultPose.trans) / glm::length(fbxZeroTrans); } AnimPose trans = AnimPose(glm::vec3(1.0f), glm::quat(), relDefaultPose.trans + boneLengthScale * (fbxAnimTrans - fbxZeroTrans)); _anim[frame][skeletonJoint] = trans * preRot * rot * postRot; } } } // mirrorAnim will be re-built on demand, if needed. _mirrorAnim.clear(); _poses.resize(skeletonJointCount); }
void draw(const Eigen::Quaternionf &q, const Eigen::Vector3f &translation, const cv::Scalar &color, std::list<cv::Point2f> &tail, bool draw_tail = true) { // Project z axis to ground plane Eigen::Vector3f z(0.0, 0.0, 1.0); z = q.matrix() * z + translation; // Metric dimension in 3D coordinate frame float d_x = 15.24; // In meter float d_y = 28.6512; // In meter // Convert to image coordinate float s_x = img_draw.cols; float s_y = img_draw.rows; float o_x = s_x - translation(1) / d_y * s_x; float o_y = s_y - translation(0) / d_x * s_y; float p_x = s_x - z(1) / d_y * s_x; float p_y = s_y - z(0) / d_x * s_y; float s = sqrt((p_x - o_x) * (p_x - o_x) + (p_y - o_y) * (p_y - o_y)); float u = (p_x - o_x) / s; float v = (p_y - o_y) / s; float theta = atan2(v, u); // 2D rotation matrix and translation Eigen::Rotation2Df rot(theta); Eigen::Vector2f t(o_x, o_y); // Camera cone std::vector<Eigen::Vector2f> camera; float l = 5; Eigen::Vector2f o = rot * Eigen::Vector2f(0.0, 0.0) + t; Eigen::Vector2f a = rot * Eigen::Vector2f(3 * l, -3 * l) + t; Eigen::Vector2f b = rot * Eigen::Vector2f(3 * l, 3 * l) + t; if (tail.size() == 250) tail.pop_front(); tail.push_back(cv::Point2f(o(0), o(1))); cv::circle(img_draw, cv::Point2f(o_x, o_y), 3, color); cv::line(img_draw, cv::Point2f(o(0), o(1)), cv::Point2f(a(0), a(1)), color, 2, CV_AA); cv::line(img_draw, cv::Point2f(o(0), o(1)), cv::Point2f(b(0), b(1)), color, 2, CV_AA); cv::line(img_draw, cv::Point2f(a(0), a(1)), cv::Point2f(b(0), b(1)), color, 2, CV_AA); if (true == draw_tail) { cv::Point2f p1, p2; for (std::list<cv::Point2f>::iterator iter = tail.begin(); iter != tail.end(); ++iter) { if (iter == tail.begin()) { p1 = *iter; continue; } p2 = *iter; cv::line(img_draw, p1, p2, color, 2, CV_AA); p1 = p2; } } }
Edge_Record Edge_Record::rprev() const { return rot().onext().rot_inv(); }
bool OgreModel::fromJson(const Json::Value &node, Ogre::SceneNode *levelRoot, Ogre::SceneManager *sceneManager, const Ogre::String &resourceGroupName) { // data to gather Ogre::String meshName; Ogre::Vector3 pos(Ogre::Vector3::ZERO); Ogre::Quaternion rot(Ogre::Radian(0), -Ogre::Vector3::UNIT_Z); Ogre::Vector3 scale(Ogre::Vector3::UNIT_SCALE); Json::Value value; bool allWasFine = true; // position if(node[OgreModel::POSITION_ATTRIBUTE].isNull()) Debug::error(STEEL_METH_INTRO, "position is null: no translation applied.").endl(); else pos = JsonUtils::asVector3(node[OgreModel::POSITION_ATTRIBUTE], pos); //rotation if(!node[OgreModel::ROTATION_ATTRIBUTE].isNull()) rot = JsonUtils::asQuaternion(node[OgreModel::ROTATION_ATTRIBUTE], rot); //scale if(!node[OgreModel::SCALE_ATTRIBUTE].isNull()) scale = JsonUtils::asVector3(node[OgreModel::SCALE_ATTRIBUTE], scale); // mesh name value = node[OgreModel::ENTITY_MESH_NAME_ATTRIBUTE]; if(value.isNull() && !(allWasFine = false)) Debug::error(STEEL_METH_INTRO, "field ").quotes(OgreModel::ENTITY_MESH_NAME_ATTRIBUTE)(" is null.").endl(); else meshName = Ogre::String(value.asString()); // custom material Ogre::String materialName = StringUtils::BLANK; if(node.isMember(OgreModel::MATERIAL_OVERRIDE_ATTRIBUTE)) { value = node[OgreModel::MATERIAL_OVERRIDE_ATTRIBUTE]; materialName = JsonUtils::asString(value, StringUtils::BLANK); if(StringUtils::BLANK == materialName) Debug::error(STEEL_METH_INTRO, "could not read field ").quotes(OgreModel::MATERIAL_OVERRIDE_ATTRIBUTE)(": ").quotes(value).endl(); } // agentTags allWasFine &= deserializeTags(node); if(!allWasFine) { Debug::error("json was:").endl()(node.toStyledString()).endl(); Debug::error("model deserialisation aborted.").endl(); return false; } // now whether we have minor changes (and we apply them directly), or major ones (cleanup, then init). // lets start with major ones if(mEntity == nullptr || meshName != mEntity->getMesh()->getName()) { // make sure we've been called with all arguments, because they're all needed now if(levelRoot == nullptr || sceneManager == nullptr) { Debug::error(STEEL_METH_INTRO, "a new mesh is required, but sceneManager or levelRoot are nullptr. Aborting.").endl(); return false; } // make sure the new meshName is valid if(meshName.length() == 0 || !Ogre::ResourceGroupManager::getSingletonPtr()->resourceExistsInAnyGroup(meshName)) { Debug::error(STEEL_METH_INTRO, "could not find resource ").quotes(meshName)(" in any group. Aborting.").endl(); return false; } if(!init(meshName, pos, rot, scale, levelRoot, sceneManager, resourceGroupName)) { return false; } } else { setPosition(pos); setRotation(rot); setScale(scale); } if(StringUtils::BLANK != materialName) { setMaterial(materialName, resourceGroupName); } return true; }
void laserPclCallback(const sensor_msgs::PointCloud2::ConstPtr &msg) { pcl::PCLPointCloud2 pcl_pcl2; ros::Duration t(0.01); static int counter = 0; // Convert the sensor_msgs/PointCloud2 data to pcl/PointCloud pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ> ); pcl::fromROSMsg (*msg, *cloud); // processing here tf::TransformListener tf_listener_; // Obtain sensor_pose Eigen::Affine3f sensorPose; // Eigen::Affine3f sensorPose = Eigen::Affine3f(Eigen::Translation3f(cloud->sensor_origin_[0], // cloud->sensor_origin_[1], // cloud->sensor_origin_[2])) * // Eigen::Affine3f(cloud->sensor_orientation_); tf::StampedTransform transform; try { tf_listener_.waitForTransform("base_link", "laser", ros::Time::now(), ros::Duration(1.0)); tf_listener_.lookupTransform("base_link", "laser", ros::Time(0), transform); } catch ( std::runtime_error e ) { ROS_ERROR("%s",e.what()); } tf::Vector3 p = transform.getOrigin(); tf::Quaternion q = transform.getRotation(); sensorPose = (Eigen::Affine3f)Eigen::Translation3f(p.getX(), p.getY(), p.getZ()); Eigen::Quaternion<float> rot(q.getW(), q.getX(), q.getY(), q.getZ()); sensorPose = sensorPose * rot; // if (tx != 0.0) // { // Eigen::Affine3f trans = // (Eigen::Affine3f)Eigen::Translation3f(-tx/fx , 0, 0); // sensorPose = sensorPose * trans; // } pcl::RangeImagePlanar range_image_planar; range_image_planar.createFromPointCloudWithFixedSize(*cloud, imageSizeX, imageSizeY, centerX, centerY, focalLengthX, focalLengthY, sensorPose, pcl::RangeImage::LASER_FRAME, noiseLevel, minRange); // // -------------------------- // // -----Show range image----- // // -------------------------- cv::Mat img(range_image_planar.height, range_image_planar.width, CV_32F); for (int i = 0; i < imageSizeY; i++) for (int j = 0; j < imageSizeX; j++) { const pcl::PointWithRange p = range_image_planar.getPoint(i * imageSizeX + j) ; // ROS_INFO("pixel (%d %d) = %f ", i, j, p.z); img.at<float>(i * imageSizeX + j ) = p.x; // if (p.z > LASER_RANGE_MAX) // img.at<float>(i * imageSizeX + j ) = LASER_RANGE_MAX; } double min, max; cv::minMaxIdx(img, &min, &max); ROS_INFO("max = %f min= %f", max, min); //if (min<0)min = min*-1; // ROS_INFO("min: %f", fabs(min)); // Add minimum values // cv::Mat img_corrected(range_image_planar.height, range_image_planar.width, CV_32F); for (int i = 0; i < imageSizeY; i++) for (int j = 0; j < imageSizeX; j++) { const pcl::PointWithRange p = range_image_planar.getPoint(i * imageSizeX + j) ; // ROS_INFO("pixel (%d %d) = %f ", i, j, p.z); img.at<float>(i * imageSizeX + j ) = p.x - min; // if (p.z > LASER_RANGE_MAX) // img.at<float>(i * imageSizeX + j ) = LASER_RANGE_MAX; } cv::Mat bwimg; cv::convertScaleAbs(img, bwimg, 255/max); //(max - min)); cv::Mat gaussimg = bwimg.clone(); cv::GaussianBlur(bwimg, gaussimg, cv::Size(3, 3), 0, 0); // cv::Mat proba; // cv::convertScaleAbs(img_corrected, proba, 255/(max - min)); // cv::Mat gaussimg_2 = proba.clone(); // cv::GaussianBlur(proba, gaussimg_2, cv::Size(3, 3), 0, 0); // if (counter % 2 == 0) // { //ROS_INFO("Counter = %d", counter); viewer.showRangeImage(range_image_planar); imshow("depth", gaussimg); //imshow("corrected", gaussimg_2); // } // counter = counter + 1; // cv::Mat img(range_image.width, range_image.height, CV_32F); // for (size_t i = 0; i < (range_image.points.size()); i++) // { // float z = range_image.getPoint(i).z; // img.at<float>(i)=z; // } // imshow("depth", img); cv::waitKey(1); // // Convert to ROS data type // sensor_msgs::PointCloud2 output; // pcl_conversions::fromPCL(cloud_filtered, output); // // Publish the data. // pub.publish (output); viewer.spinOnce (); pcl_sleep (0.01); //t.sleep(); }
void AlchimedesBuoyancy(DemoEntityManager* const scene) { // load the sky box scene->CreateSkyBox(); // load the mesh CreateLevelMesh (scene, "swimmingPool.ngd", true); // add a trigger Manager to the world MyTriggerManager* const triggerManager = new MyTriggerManager(scene->GetNewton()); dMatrix triggerLocation (dGetIdentityMatrix()); triggerLocation.m_posit.m_x = 17.0f; triggerLocation.m_posit.m_y = -3.5f; NewtonCollision* const poolBox = NewtonCreateBox (scene->GetNewton(), 30.0f, 6.0f, 20.0f, 0, NULL); triggerManager->CreateBuoyancyTrigger (triggerLocation, poolBox); NewtonDestroyCollision (poolBox); // customize the scene after loading // set a user friction variable in the body for variable friction demos // later this will be done using LUA script dMatrix offsetMatrix (dGetIdentityMatrix()); // place camera into position dMatrix camMatrix (dGetIdentityMatrix()); dQuaternion rot (camMatrix); dVector origin (-20.0f, 10.0f, 0.0f, 0.0f); scene->SetCameraMatrix(rot, origin); int defaultMaterialID = NewtonMaterialGetDefaultGroupID (scene->GetNewton()); /* //test buoyancy on scaled collisions dVector plane (0.0f, 1.0f, 0.0f, 0.0f); dMatrix L1 (dPitchMatrix(30.0f * 3.141692f / 180.0f) * dYawMatrix(0.0f * 3.141692f / 180.0f) * dRollMatrix(0.0f * 3.141692f / 180.0f)); NewtonCollision* xxx0 = NewtonCreateCompoundCollision(scene->GetNewton(), 0); NewtonCompoundCollisionBeginAddRemove(xxx0); NewtonCollision* xxxx0 = NewtonCreateBox(scene->GetNewton(), 1.0f, 2.0f, 1.0f, 0, &L1[0][0]); NewtonCompoundCollisionAddSubCollision(xxx0, xxxx0); NewtonCompoundCollisionEndAddRemove(xxx0); NewtonCollision* xxx1 = NewtonCreateCompoundCollision(scene->GetNewton(), 0); NewtonCollision* xxxx1 = NewtonCreateBox(scene->GetNewton(), 1.0f, 1.0f, 1.0f, 0, &L1[0][0]); NewtonCompoundCollisionAddSubCollision(xxx1, xxxx1); NewtonCompoundCollisionEndAddRemove(xxx1); NewtonCollisionSetScale(xxx1, 1.0f, 2.0f, 1.0f); //dMatrix m (dPitchMatrix(45.0f * 3.141692f / 180.0f) * dYawMatrix(40.0f * 3.141692f / 180.0f) * dRollMatrix(70.0f * 3.141692f / 180.0f)); dMatrix m (dPitchMatrix(0.0f * 3.141692f / 180.0f) * dYawMatrix(0.0f * 3.141692f / 180.0f) * dRollMatrix(0.0f * 3.141692f / 180.0f)); dVector gravity (0.0f, 0.0f, -9.8f, 0.0f); dVector cog0 (0.0f, 0.0f, 0.0f, 0.0f); dVector accelPerUnitMass0; dVector torquePerUnitMass0; NewtonConvexCollisionCalculateBuoyancyAcceleration (xxx0, &m[0][0], &cog0[0], &gravity[0], &plane[0], 1.0f, 0.1f, &accelPerUnitMass0[0], &torquePerUnitMass0[0]); dVector cog1 (0.0f, 0.0f, 0.0f, 0.0f); dVector accelPerUnitMass1; dVector torquePerUnitMass1; NewtonConvexCollisionCalculateBuoyancyAcceleration (xxx1, &m[0][0], &cog1[0], &gravity[0], &plane[0], 1.0f, 0.1f, &accelPerUnitMass1[0], &torquePerUnitMass1[0]); */ int count = 5; dVector size (1.0f, 0.25f, 0.5f); dVector location (10.0f, 0.0f, 0.0f, 0.0f); dMatrix shapeOffsetMatrix (dGetIdentityMatrix()); // AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _SPHERE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix); AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _BOX_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix); // AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CAPSULE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix); // AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix); // AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CONE_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix); // AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _CHAMFER_CYLINDER_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix); // AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _REGULAR_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix); // AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _COMPOUND_CONVEX_CRUZ_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix); /* for (NewtonBody* bodyPtr = NewtonWorldGetFirstBody(scene->GetNewton()); bodyPtr; bodyPtr = NewtonWorldGetNextBody(scene->GetNewton(), bodyPtr)) { NewtonCollision* collision = NewtonBodyGetCollision(bodyPtr); if (NewtonCollisionGetType(collision) == SERIALIZE_ID_COMPOUND) { NewtonCollisionSetScale (collision, 0.5f, 0.5f, 0.5f); } } */ // AddPrimitiveArray(scene, 10.0f, location, size, count, count, 5.0f, _RANDOM_CONVEX_HULL_PRIMITIVE, defaultMaterialID, shapeOffsetMatrix); }
namespace MATPACK { //-----------------------------------------------------------------------------// static const float defFOV(45); static const Trafo defT (trans(0,0,-5) // view transformation matrix * rot(Vector3D(0,1,0),M_PI/5) * rot(Vector3D(1,0,0),M_PI/3) * rot(Vector3D(0,1,0),-M_PI/10) * rot(Vector3D(0,0,1),-M_PI/2) * rot(Vector3D(1,0,0),-M_PI/2)); //-----------------------------------------------------------------------------// static bool Valid2dPlotForReal2dArray (int plotype2d) { switch(plotype2d) { case MpViewGraphData::Two_Dim_Contour_Bezier_Plot: case MpViewGraphData::Two_Dim_Contour_BSpline_Plot: case MpViewGraphData::Two_Dim_Contour_Linear_Plot: case MpViewGraphData::Two_Dim_Contour_Cubic_Fill_Plot: case MpViewGraphData::Two_Dim_Contour_Cubic_Fill_Line_Plot: case MpViewGraphData::Two_Dim_Density_Plot: case MpViewGraphData::Two_Dim_Image_Linear_Plot: case MpViewGraphData::Two_Dim_Image_Linear_Diffusive_Plot: case MpViewGraphData::Two_Dim_Image_Linear_Ordered_Plot: case MpViewGraphData::Two_Dim_Image_Linear_Coarse_Plot: case MpViewGraphData::Two_Dim_Image_Bezier_Plot: case MpViewGraphData::Two_Dim_Image_Bezier_Diffusive_Plot: case MpViewGraphData::Two_Dim_Image_Bezier_Ordered_Plot: case MpViewGraphData::Two_Dim_Image_Bezier_Coarse_Plot: case MpViewGraphData::Two_Dim_Lambert_Plot: case MpViewGraphData::Two_Dim_Box_Plot_Squares: case MpViewGraphData::Two_Dim_Box_Plot_Circles: case MpViewGraphData::Two_Dim_Density_Checkerboard: return true; default: return false; } } static bool Valid2dPlotForComplex2dArray (int plotype2d) { switch(plotype2d) { case MpViewGraphData::Two_Dim_Field_Abs_Plot: case MpViewGraphData::Two_Dim_Field_Phase_Plot: case MpViewGraphData::Two_Dim_Field_Thick_Plot: return true; default: return false; } } static bool Valid3dPlotForReal2dArray (int plotype3d) { switch(plotype3d) { case MpViewGraphData::Three_Dim_Surface_Plot: case MpViewGraphData::Three_Dim_Contour_Plot: case MpViewGraphData::Three_Dim_Dots_Plot: case MpViewGraphData::Three_Dim_Poles_Plot: case MpViewGraphData::Three_Dim_Scatter_Plot: case MpViewGraphData::Three_Dim_Project_Plot: return true; default: return false; } } static bool Valid3dPlotForComplex2dArray (int plotype3d) { switch(plotype3d) { case MpViewGraphData::Three_Dim_Complex_Surface_Plot: return true; default: return false; } } static bool Valid3dPlotForXYZArray (int plotype3d) { switch(plotype3d) { case MpViewGraphData::Three_Dim_Poles_Plot: case MpViewGraphData::Three_Dim_Scatter_Plot: return true; default: return false; } } static bool Valid4dPlotForReal3dArray (int plotype4d) { switch(plotype4d) { case MpViewGraphData::Four_Dim_Contour_Plot: case MpViewGraphData::Four_Dim_Scatter_Plot: case MpViewGraphData::Four_Dim_Slice_Plot: return true; default: return false; } } //-----------------------------------------------------------------------------// // default view settings //-----------------------------------------------------------------------------// void MpView::DefaultViewForMatrix (MpViewGraphData *G) { if (!G) return; // define the 2D and 3D viewbox int nx=0, ny=0; if (G->DS().isMatrix()) { nx = G->DS().Z.Rows(); ny = G->DS().Z.Cols(); } else if (G->DS().isComplexMatrix()) { nx = G->DS().C.Rows(); ny = G->DS().C.Cols(); } double lim = 0.05; // i.e. 1 : 20 maximal // make true aspect ratios in x:y (within limits 1:20) G->vbx = G->vby = 1; G->shrink2d = 0.15; if (nx > ny) { G->vby = max((double)ny/nx,lim); } else if (ny > nx) { G->vbx = max((double)nx/ny,lim); } // flat box is nicer in most cases G->vbz = 0.5; G->viewtrafo = defT; G->fov = defFOV; } //-----------------------------------------------------------------------------// void MpView::DefaultViewForMatrix3d (MpViewGraphData *G) { if (!G) return; int nx = G->DS().V.Slices(), ny = G->DS().V.Rows(), nz = G->DS().V.Columns(); G->vbx = G->vby = G->vbz = 1.0; // make true aspect ratios in x:y:z (within limits) double lim = 0.05; // i.e. 1 : 20 maximal if (nx >= ny && nx >= nz) { G->vby = max((double)ny/nx,lim); G->vbz = max((double)nz/nx,lim); } else if (ny >= nx && ny >= nz) { G->vbx = max((double)nx/ny,lim); G->vbz = max((double)nz/ny,lim); } else if (nz >= nx && nz >= ny) { G->vbx = max((double)nx/nz,lim); G->vby = max((double)ny/nz,lim); } G->viewtrafo = defT; G->fov = defFOV; } //-----------------------------------------------------------------------------// void MpView::DefaultViewForXYZSet (MpViewGraphData *G) { if (!G) return; // get bounding box double bx1 = Min(G->DS().x), bx2 = Max(G->DS().x), nx = abs(bx2-bx1), by1 = Min(G->DS().y), by2 = Max(G->DS().y), ny = abs(by2-by1), bz1 = Min(G->DS().z), bz2 = Max(G->DS().z), nz = abs(bz2-bz1); G->vbx = G->vby = G->vbz = 1.0; // make true aspect ratios in x:y:z (within limits) double lim = 0.05; // i.e. 1 : 20 maximal if (nx >= ny && nx >= nz) { G->vby = max((double)ny/nx,lim); G->vbz = max((double)nz/nx,lim); } else if (ny >= nx && ny >= nz) { G->vbx = max((double)nx/ny,lim); G->vbz = max((double)nz/ny,lim); } else if (nz >= nx && nz >= ny) { G->vbx = max((double)nx/nz,lim); G->vby = max((double)ny/nz,lim); } G->viewtrafo = defT; G->fov = defFOV; } //-----------------------------------------------------------------------------// void MpView::DefaultViewForScene (MpViewGraphData *G) { if (!G) return; Scene& S = G->DS().S; // calculate bounding box float x1,x2,y1,y2,z1,z2; S.Extent(x1,x2,y1,y2,z1,z2); // view box ratios x:y:z const float f = 0.5; // scale factor G->vby = f*(x2-x1); // note ordering (y-z-x) - the internal coordinate system G->vbz = f*(y2-y1); G->vbx = f*(z2-z1); // choose an initial viewpoint depending on extent of scene // look to the center of the scene float cx = (x1+x2)/2, cy = (y1+y2)/2, cz = (z1+z2)/2, r = 1.7 * hypot(x1-x2,y1-y2,z1-z2); S.Look(Vector3D(cx+r,cy+r,cz+r), // define camera position Vector3D(-r,-r,-r), // the look direction FieldOfView(23)); // and the view angle fov // set transformation matrix and fov angle G->viewtrafo = S.GetMatrix(); G->fov = S.GetFieldOfView(); } //-----------------------------------------------------------------------------// // MpView::NewMatrix: // initialize when matrix is newly loaded //-----------------------------------------------------------------------------// void MpView::NewMatrix (MpViewGraphData *G) { if (!G) return; MpViewDataSet &DS = G->DS(); // load colormap if (! G->theColorMap) { string path = string(getenv(MatpackHome)) + "/cmaps/earth-512.cmp"; G->theColorMap.Load(path.c_str()); } // set data type const Matrix &Z = DS.MatrixData(); if (Z.Empty()) { MpViewDisplay::Message("MpView::NewMatrix: no data available !!"); return; } // reset axis labeling G->Axis[0].LabelMin = Z.Rlo(); // x-axis G->Axis[0].LabelMax = Z.Rhi(); G->Axis[1].LabelMin = Z.Clo(); // y-axis G->Axis[1].LabelMax = Z.Chi(); double z1 = Min(Z), z2 = Max(Z); // reset logarithmic representation G->log_z = false; // reset contours if (G->num_contour_level_2D == 0) { // if no contour levels are allocated G->num_contour_level_2D = 10; G->contour_level_2D = new MpContourLevel[G->num_contour_level_2D]; } double cstep = (z2-z1) / (G->num_contour_level_2D+1); for (int i = 0; i < G->num_contour_level_2D; i++) G->contour_level_2D[i].level = z1 + (i+1) * cstep; // reset base of needles in scatter plot G->Three_Dim_Poles_Base = z1; // reset axis labeling G->Axis[2].LabelMin = z1; // z-axis G->Axis[2].LabelMax = z2; G->colormap_legend_axis.LabelMin = z1; G->colormap_legend_axis.LabelMax = z2; DefaultViewForMatrix(G); // define the 3D viewbox if ( (G->plotdim == MpViewGraphData::Two_Dim_Plot && Valid2dPlotForReal2dArray(G->plotype2d)) || G->plotdim == MpViewGraphData::Three_Dim_Plot && Valid3dPlotForReal2dArray(G->plotype3d)) { // previous settings are left unchanged } else { if (Z.Elements() > 10000) { G->plotdim = MpViewGraphData::Two_Dim_Plot; G->plotype2d = MpViewGraphData::Two_Dim_Image_Linear_Plot; } else if (Z.Elements() > 1600) { G->plotdim = MpViewGraphData::Two_Dim_Plot; G->plotype2d = MpViewGraphData::Two_Dim_Contour_Bezier_Plot; } else { G->plotdim = MpViewGraphData::Three_Dim_Plot; G->plotype3d = MpViewGraphData::Three_Dim_Surface_Plot; } } // check if only one row or column given, in this case no 2d plots are // available and in 3d-mode upto now only squares are available if (Z.Rhi() <= Z.Rlo() || Z.Chi() <= Z.Clo()) { G->plotdim = MpViewGraphData::Three_Dim_Plot; G->plotype3d = MpViewGraphData::Three_Dim_Surface_Plot; } } //-----------------------------------------------------------------------------// // MpView::NewComplexMatrix: // initialize when complex matrix is newly loaded //-----------------------------------------------------------------------------// void MpView::NewComplexMatrix (MpViewGraphData *G, const ComplexMatrix &New) { if (!G) return; MpViewDataSet &DS = G->DS(); // load periodic colormap - only if there is not already a colormap if (! G->theColorMap) { string path = string(getenv(MatpackHome)) + "/cmaps/prism-256.cmp"; G->theColorMap.Load(path.c_str()); } // set data type const ComplexMatrix &C = DS.ComplexMatrixData(); if (C.Empty()) { MpViewDisplay::Message("MpView::NewComplexMatrix: no data available !!"); return; } if ( (G->plotdim == MpViewGraphData::Two_Dim_Plot && Valid2dPlotForComplex2dArray(G->plotype2d)) || G->plotdim == MpViewGraphData::Three_Dim_Plot && Valid3dPlotForComplex2dArray(G->plotype3d)) { // previous settings are left unchanged } else { G->plotdim = MpViewGraphData::Three_Dim_Plot; G->plotype3d = MpViewGraphData::Three_Dim_Complex_Surface_Plot; G->complex_representation = MpViewGraphData::Cplx_Norm; } // reset axis labeling G->Axis[0].LabelMin = C.Rlo(); // x-axis G->Axis[0].LabelMax = C.Rhi(); G->Axis[1].LabelMin = C.Clo(); // y-axis G->Axis[1].LabelMax = C.Chi(); G->Axis[2].LabelMin = norm(Min(C)); // z-axis G->Axis[2].LabelMax = norm(Max(C)); G->colormap_legend_axis.LabelMin = G->Axis[2].LabelMin; G->colormap_legend_axis.LabelMax = G->Axis[2].LabelMax; DefaultViewForMatrix(G); // define the 3D viewbox } //----------------------------------------------------------------------------// void MpView::NewVolume (MpViewGraphData *G) { if (!G) return; MpViewDataSet &DS = G->DS(); // load colormap if (! G->theColorMap) { string path = string(getenv(MatpackHome)) + "/cmaps/prism-256.cmp"; G->theColorMap.Load(path.c_str()); } // set data type const MpMatrix3d<double> &V = DS.Matrix3dData(); if (V.Empty()) { MpViewDisplay::Message("MpView::InitializeForVolume: no data available !!"); return; } if (G->plotdim == MpViewGraphData::Four_Dim_Plot && Valid4dPlotForReal3dArray(G->plotype4d)) { // previous settings are left unchanged } else { G->plotdim = MpViewGraphData::Four_Dim_Plot; G->plotype4d = MpViewGraphData::Four_Dim_Contour_Plot; } // lower and upper bound for 4d scatter plot double maxi = Max(V), mini = Min(V); G->Four_Dim_Scatter_Lower = maxi-0.1*(maxi-mini); G->Four_Dim_Scatter_Upper = maxi; // computation of a 75%-quantile seems to be a good startup value for the // surface level, i.e. 75% of the volume is below this value double qq = 0, sdev = 0; // catch n <= 2 case to avoid error exit of Quantile function if (V.Elements() <= 2) qq = 0.5*(maxi+mini); else MpQuantile(V, 2, 0.75, qq, sdev); // set this value for all isosurfaces for (int i = 0; i < MpViewIsosurfaceData::MaxIsosurf; i++) G->IsoSurface[i].Four_Dim_Isosurface_Level = qq; // activate first one G->IsoSurface[0].Four_Dim_Isosurface_Active = true; G->Axis[0].LabelMin = V.Slo(); // x-axis G->Axis[0].LabelMax = V.Shi(); G->Axis[1].LabelMin = V.Rlo(); // y-axis G->Axis[1].LabelMax = V.Rhi(); G->Axis[2].LabelMin = V.Clo(); // z-axis G->Axis[2].LabelMax = V.Chi(); // define the 3D viewbox DefaultViewForMatrix3d(G); } //----------------------------------------------------------------------------// void MpView::NewXYZSet (MpViewGraphData *G) { if (!G) return; MpViewDataSet &DS = G->DS(); // load colormap if (! G->theColorMap) { string path = string(getenv(MatpackHome)) + "/cmaps/prism-256.cmp"; G->theColorMap.Load(path.c_str()); } // set data type DS.theDataType = MpViewDataSet::a_XYZSet; if (DS.x.Empty() || DS.y.Empty() || DS.z.Empty()) { MpViewDisplay::Message("MpView::NewXYZSet: no data available !!"); return; } if (G->plotdim == MpViewGraphData::Three_Dim_Plot && Valid3dPlotForXYZArray(G->plotype3d)) { // previous settings are left unchanged } else { G->plotdim = MpViewGraphData::Three_Dim_Plot; G->plotype3d = MpViewGraphData::Three_Dim_Scatter_Plot; } // define the 3D viewbox DefaultViewForXYZSet(G); } //----------------------------------------------------------------------------// // MpView::InitializeForScene // initialize several variables when scene has changed //----------------------------------------------------------------------------// void MpView::NewScene (MpViewGraphData *G) { if (!G) return; MpViewDataSet &DS = G->DS(); // load periodic colormap - only if there is not already a colormap if (! G->theColorMap) { string path = string(getenv(MatpackHome)) + "/cmaps/prism-256.cmp"; G->theColorMap.Load(path.c_str()); } // set data type Scene& S = DS.SceneData(); G->plotdim = MpViewGraphData::Three_Dim_Plot; float x1,x2,y1,y2,z1,z2; // bounding box S.Extent(x1,x2,y1,y2,z1,z2); // set clipping volume G->cx1 = x1; G->cx2 = x2; G->cy1 = y1; G->cy2 = y2; G->cz1 = z1; G->cz2 = z2; // define the 3D viewbox DefaultViewForScene(G); #ifdef DEBUG cout << "Scene["<<x1<<","<<x2<<"," <<y1<<","<<y2<<"," <<z1<<","<<z2<<"]"<<endl; #endif } //----------------------------------------------------------------------------// // If graph_index < 0 find all graphs that link into the selected data set and // switch to the default settings. // If graph_index > 0 then change settings only for the selected graph. //----------------------------------------------------------------------------// void MpView::NewDataSet (int dataset_index, int graph_index) { if (dataset_index < 0) return; for (MpViewGraphMap::iterator i = Graphs.begin(); i != Graphs.end(); ++i) { int grf = i->first; if (dataset_index == i->second.iset && (graph_index < 0 || graph_index == grf)) { MpViewGraphData *G = &GD(grf); if (DataSetExists(dataset_index)) { switch (G->DS().GetDataType()) { case MpViewDataSet::a_EmptySet: // nothing to do break; case MpViewDataSet::a_Matrix: NewMatrix(G); break; case MpViewDataSet::a_Matrix3d: NewVolume(G); break; case MpViewDataSet::a_ComplexMatrix: NewComplexMatrix(G,G->DS().C); break; case MpViewDataSet::a_Scene: NewScene(G); break; case MpViewDataSet::a_XYZSet: NewXYZSet(G); break; case MpViewDataSet::a_Image: cerr<<"MpView::NewDataSet: IMAGE - can't happen"<<endl; break; default: cerr<<"MpView::NewDataSet: found illegal data set type" << endl; break; } } else { // data set doesn't exist (e.g. was deleted) -> unlink G->iset = -1; } } } } } // namespace MATPACK