Example #1
1
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;
}
Example #3
0
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);
}
Example #6
0
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");
    }
}
Example #7
0
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();
}
Example #10
0
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);
}
Example #11
0
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;
  }
  }
}
Example #12
0
Edge_Record Edge_Record::rprev() const {
    return rot().onext().rot_inv();
}
Example #13
0
    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;
    }
Example #14
0
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);
}
Example #16
0
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