<commit_before>/*
 * Copyright © 2012, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All rights reserved.
 *
 * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed
 * under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0.
 *
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
 * either express or implied. See the License for the specific language
 * governing permissions and limitations under the License.
*/

/**
 * @file CaterpillarModel.cpp
 * @brief Contains the implementation of class CaterpillarModel.
 * $Id$
 */

// This module
#include "CaterpillarModel.h"
// This library
#include "core/tgLinearString.h"
#include "core/tgRod.h"
#include "core/tgString.h"
#include "tgcreator/tgBuildSpec.h"
#include "tgcreator/tgLinearStringInfo.h"
#include "tgcreator/tgRodInfo.h"
#include "tgcreator/tgStructure.h"
#include "tgcreator/tgStructureInfo.h"
// The Bullet Physics library
#include "LinearMath/btVector3.h"
// The C++ Standard Library
#include <stdexcept>

namespace
{
    // see tgBaseString.h for a descripton of some of these rod parameters
    // (specifically, those related to the motor moving the strings.)
    // NOTE that any parameter that depends on units of length will scale
    // with the current gravity scaling. E.g., with gravity as 98.1,
    // the length units below are in decimeters.

    // Note: This current model of the SUPERball rod is 1.5m long by 3 cm radius,
    // which is 0.00424 m^3.
    // For SUPERball v1.5, mass = 3.5kg per strut, which comes out to
    // 0.825 kg / (decimeter^3).

    // similarly, frictional parameters are for the tgRod objects.
    const struct Config
    {
        double density;
        double radius;
        double stiffness;
        double damping;
        double rod_length;
        double rod_space;
        double friction;
        double rollFriction;
        double restitution;
        bool   hist;
        double rotation;
        double maxTens;
        double targetVelocity;
        double maxAcc;
    } c =
   {
     0.688,    // density (kg / length^3)
     0.31,     // radius (length)
     613.0,   // stiffness (kg / sec^2) was 1500
     200.0,    // damping (kg / sec)
     16.84,     // rod_length (length)
     7.5,      // rod_space (length)
     0.99,      // friction (unitless)
     0.01,     // rollFriction (unitless)
     0.0,      // restitution (?)
     0,			// History logging (boolean)
     0,        // rotation
     100000,   // maxTens
     10000,    // targetVelocity
     20000     // maxAcc

     // Use the below values for earlier versions of simulation.
     // 1.006,
     // 0.31,
     // 300000.0,
     // 3000.0,
     // 15.0,
     // 7.5,
  };

  void addSegments(tgStructure& snake, const tgStructure& tetra, double edge,
             size_t segmentCount)
    {

        const btVector3 offset(-edge, 0, 0.0);
        for (size_t i = 0; i < segmentCount; ++i)
        {
            tgStructure* const t = new tgStructure(tetra);
            t->addTags(tgString("segment", i + 1));
            t->move((i + 1) * offset);
            // Add a child to the snake
            snake.addChild(t);
        }
    }

      // Add muscles that connect the segments
    void addSegmentMuscles(tgStructure& snake)
    {
        const std::vector<tgStructure*> children = snake.getChildren();
            for (size_t i = 1; i < children.size(); ++i)
            {
                tgNodes n0 = children[i-1]->getNodes();
                tgNodes n1 = children[i  ]->getNodes();

				tgNode mid0((n0[0] + n1[0])/2.0);
				tgNode mid1((n0[1] + n1[1])/2.0);
				tgNode mid2((n0[2] + n1[2])/2.0);
				tgNode mid3((n0[3] + n1[3])/2.0);
#if (0)
				if (i == 1)
				{
					btScalar segDist = (n0[0] - n1[0]).length() / 2.0;
					tgNode start0 = n0[0] + btVector3(segDist, 0.0, 0.0);
					tgNode start1 = n0[1] + btVector3(segDist, 0.0, 0.0);
					tgNode start2 = n0[2] + btVector3(segDist, 0.0, 0.0);
					tgNode start3 = n0[3] + btVector3(segDist, 0.0, 0.0);

					snake.addPair(start0, start1, "segment rod");
					snake.addPair(start2, start3, "segment rod");

					snake.addPair(n0[5], start0, "segment muscle front one");
					snake.addPair(n0[5], start2, "segment muscle front two");
					snake.addPair(n0[7], start1, "segment muscle front three");
					snake.addPair(n0[7], start3, "segment muscle front four");

					snake.addPair(n0[8], start0, "segment muscle rear one");
					snake.addPair(n0[8], start1, "segment muscle rear two");
					snake.addPair(n0[9], start2, "segment muscle rear three");
					snake.addPair(n0[9], start3, "segment muscle rear four");

				}
				if (i == children.size() - 1)
				{
					btScalar segDist = (n0[0] - n1[0]).length() / 2.0;
					tgNode end0 = n1[0] - btVector3(segDist, 0.0, 0.0);
					tgNode end1 = n1[1] - btVector3(segDist, 0.0, 0.0);
					tgNode end2 = n1[2] - btVector3(segDist, 0.0, 0.0);
					tgNode end3 = n1[3] - btVector3(segDist, 0.0, 0.0);

					snake.addPair(end0, end1, "segment rod");
					snake.addPair(end2, end3, "segment rod");

					snake.addPair(n1[4], end0, "segment muscle rear one");
					snake.addPair(n1[4], end2, "segment muscle rear two");
					snake.addPair(n1[6], end1, "segment muscle rear three");
					snake.addPair(n1[6], end3, "segment muscle rear four");

					snake.addPair(n1[10], end0, "segment muscle rear one");
					snake.addPair(n1[10], end1, "segment muscle rear two");
					snake.addPair(n1[11], end2, "segment muscle rear three");
					snake.addPair(n1[11], end3, "segment muscle rear four");
				}
#endif
				snake.addPair(mid0, mid1, "segment rod");
				snake.addPair(mid2, mid3, "segment rod");

                snake.addPair(n0[4], mid0, "segment muscle rear one");
                snake.addPair(n0[4], mid2, "segment muscle rear two");
                snake.addPair(n0[6], mid1, "segment muscle rear three");
                snake.addPair(n0[6], mid3, "segment muscle rear four");

                snake.addPair(n1[5], mid0, "segment muscle front one");
                snake.addPair(n1[5], mid2, "segment muscle front two");
                snake.addPair(n1[7], mid1, "segment muscle front three");
                snake.addPair(n1[7], mid3, "segment muscle front four");

                snake.addPair(n0[10], mid0, "segment muscle rear one");
                snake.addPair(n0[10], mid1, "segment muscle rear two");
                snake.addPair(n0[11], mid2, "segment muscle rear three");
                snake.addPair(n0[11], mid3, "segment muscle rear four");

                snake.addPair(n1[8], mid0, "segment muscle rear one");
                snake.addPair(n1[8], mid1, "segment muscle rear two");
                snake.addPair(n1[9], mid2, "segment muscle rear three");
                snake.addPair(n1[9], mid3, "segment muscle rear four");

            }
    }
} // namespace

CaterpillarModel::CaterpillarModel() : tgModel()
{
}

CaterpillarModel::~CaterpillarModel()
{
}

void CaterpillarModel::addNodes(tgStructure& s)
{
    const double half_length = c.rod_length / 2;

    s.addNode(-c.rod_space,  -half_length, 0);            // 0
    s.addNode(-c.rod_space,   half_length, 0);            // 1
    s.addNode( c.rod_space,  -half_length, 0);            // 2
    s.addNode( c.rod_space,   half_length, 0);            // 3
    s.addNode(0,           -c.rod_space,   -half_length); // 4
    s.addNode(0,           -c.rod_space,    half_length); // 5
    s.addNode(0,            c.rod_space,   -half_length); // 6
    s.addNode(0,            c.rod_space,    half_length); // 7
    s.addNode(-half_length, 0,            c.rod_space);   // 8
    s.addNode( half_length, 0,            c.rod_space);   // 9
    s.addNode(-half_length, 0,           -c.rod_space);   // 10
    s.addNode( half_length, 0,           -c.rod_space);   // 11
}

void CaterpillarModel::addRods(tgStructure& s)
{
    s.addPair( 0,  1, "y rod");
    s.addPair( 2,  3, "y rod");
    s.addPair( 4,  5, "z rod");
    s.addPair( 6,  7, "z rod");
    s.addPair( 8,  9, "x rod");
    s.addPair(10, 11, "x rod");
}

void CaterpillarModel::addMuscles(tgStructure& s)
{
    s.addPair(0, 4,  "muscle");
    s.addPair(0, 5,  "muscle");
    s.addPair(0, 8,  "muscle");
    s.addPair(0, 10, "muscle");

    s.addPair(1, 6,  "muscle");
    s.addPair(1, 7,  "muscle");
    s.addPair(1, 8,  "muscle");
    s.addPair(1, 10, "muscle");

    s.addPair(2, 4,  "muscle");
    s.addPair(2, 5,  "muscle");
    s.addPair(2, 9,  "muscle");
    s.addPair(2, 11, "muscle");

    s.addPair(3, 7,  "muscle");
    s.addPair(3, 6,  "muscle");
    s.addPair(3, 9,  "muscle");
    s.addPair(3, 11, "muscle");

    s.addPair(4, 2,  "muscle");
    s.addPair(4, 10, "muscle");
    s.addPair(4, 11, "muscle");

    s.addPair(5, 8,  "muscle");
    s.addPair(5, 9,  "muscle");

    s.addPair(6, 10, "muscle");
    s.addPair(6, 11, "muscle");

    s.addPair(7, 8,  "muscle");
    s.addPair(7, 9,  "muscle");

}

void CaterpillarModel::setup(tgWorld& world)
{

    const tgRod::Config rodConfig(c.radius, c.density, c.friction,
				c.rollFriction, c.restitution);

    tgLinearString::Config muscleConfig(c.stiffness, c.damping, c.hist, c.rotation,
					    c.maxTens, c.targetVelocity,
					    c.maxAcc);

    // Start creating the structure
    tgStructure s;
    addNodes(s);
    addRods(s);
    addMuscles(s);

    btVector3 rotationPoint = btVector3(0, 0, 0); // origin
    btVector3 rotationAxis = btVector3(0, 1, 0);  // y-axis
    double rotationAngle = M_PI/2;
    s.addRotation(rotationPoint, rotationAxis, rotationAngle);

    s.move(btVector3(0, 10, 0));

	tgStructure caterpillar;
	addSegments(caterpillar, s, 2.0 * c.rod_length, 5);
	addSegmentMuscles(caterpillar);

    // Create the build spec that uses tags to turn the structure into a real model
    tgBuildSpec spec;
    spec.addBuilder("rod", new tgRodInfo(rodConfig));
    spec.addBuilder("muscle", new tgLinearStringInfo(muscleConfig));

    // Create your structureInfo
    tgStructureInfo structureInfo(caterpillar, spec);

    // Use the structureInfo to build ourselves
    structureInfo.buildInto(*this, world);

    // We could now use tgCast::filter or similar to pull out the
    // models (e.g. muscles) that we want to control.
    allMuscles = tgCast::filter<tgModel, tgLinearString> (getDescendants());

    // call the onSetup methods of all observed things e.g. controllers
    notifySetup();

    // Actually setup the children
    tgModel::setup(world);
}

void CaterpillarModel::step(double dt)
{
    // Precondition
    if (dt <= 0.0)
    {
        throw std::invalid_argument("dt is not positive");
    }
    else
    {
        // Notify observers (controllers) of the step so that they can take action
        notifyStep(dt);
        tgModel::step(dt);  // Step any children
    }
}

void CaterpillarModel::onVisit(tgModelVisitor& r)
{
    tgModel::onVisit(r);
}

const std::vector<tgLinearString*>& CaterpillarModel::getAllMuscles() const
{
    return allMuscles;
}

void CaterpillarModel::teardown()
{
    tgModel::teardown();
}
<commit_msg>new attempt to connect with rigids, will do muscles later<commit_after>/*
 * Copyright © 2012, United States Government, as represented by the
 * Administrator of the National Aeronautics and Space Administration.
 * All rights reserved.
 *
 * The NASA Tensegrity Robotics Toolkit (NTRT) v1 platform is licensed
 * under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * http://www.apache.org/licenses/LICENSE-2.0.
 *
 * Unless required by applicable law or agreed to in writing,
 * software distributed under the License is distributed on an
 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND,
 * either express or implied. See the License for the specific language
 * governing permissions and limitations under the License.
*/

/**
 * @file CaterpillarModel.cpp
 * @brief Contains the implementation of class CaterpillarModel.
 * $Id$
 */

// This module
#include "CaterpillarModel.h"
// This library
#include "core/tgLinearString.h"
#include "core/tgRod.h"
#include "core/tgString.h"
#include "tgcreator/tgBuildSpec.h"
#include "tgcreator/tgLinearStringInfo.h"
#include "tgcreator/tgRodInfo.h"
#include "tgcreator/tgStructure.h"
#include "tgcreator/tgStructureInfo.h"
// The Bullet Physics library
#include "LinearMath/btVector3.h"
// The C++ Standard Library
#include <stdexcept>

namespace
{
    // see tgBaseString.h for a descripton of some of these rod parameters
    // (specifically, those related to the motor moving the strings.)
    // NOTE that any parameter that depends on units of length will scale
    // with the current gravity scaling. E.g., with gravity as 98.1,
    // the length units below are in decimeters.

    // Note: This current model of the SUPERball rod is 1.5m long by 3 cm radius,
    // which is 0.00424 m^3.
    // For SUPERball v1.5, mass = 3.5kg per strut, which comes out to
    // 0.825 kg / (decimeter^3).

    // similarly, frictional parameters are for the tgRod objects.
    const struct Config
    {
        double density;
        double radius;
        double stiffness;
        double damping;
        double rod_length;
        double rod_space;
        double friction;
        double rollFriction;
        double restitution;
        bool   hist;
        double rotation;
        double maxTens;
        double targetVelocity;
        double maxAcc;
    } c =
   {
     0.688,    // density (kg / length^3)
     0.31,     // radius (length)
     613.0,   // stiffness (kg / sec^2) was 1500
     200.0,    // damping (kg / sec)
     16.84,     // rod_length (length)
     7.5,      // rod_space (length)
     0.99,      // friction (unitless)
     0.01,     // rollFriction (unitless)
     0.0,      // restitution (?)
     0,			// History logging (boolean)
     0,        // rotation
     100000,   // maxTens
     10000,    // targetVelocity
     20000     // maxAcc

     // Use the below values for earlier versions of simulation.
     // 1.006,
     // 0.31,
     // 300000.0,
     // 3000.0,
     // 15.0,
     // 7.5,
  };

  void addSegments(tgStructure& snake, const tgStructure& tetra, double edge,
             size_t segmentCount)
    {

        const btVector3 offset(-edge, 0, 0.0);
        for (size_t i = 0; i < segmentCount; ++i)
        {
            tgStructure* const t = new tgStructure(tetra);
            t->addTags(tgString("segment", i + 1));
            t->move((i + 1) * offset);
            // Add a child to the snake
            snake.addChild(t);
        }
    }

      // Add muscles that connect the segments
    void addSegmentMuscles(tgStructure& snake)
    {
        const std::vector<tgStructure*> children = snake.getChildren();

		for (size_t i = 1; i < children.size(); ++i)
		{
			    tgNodes n0 = children[i-1]->getNodes();
                tgNodes n1 = children[i  ]->getNodes();

                if (i % 2 == 0)
                {
					snake.addPair(n0[4], n1[5], "segment rod");
				}
				else
				{
					snake.addPair(n0[6], n1[7], "segment rod");
				}
		}
	}

      // Add muscles that connect the segments
    void addSegmentMusclesOld(tgStructure& snake)
    {
        const std::vector<tgStructure*> children = snake.getChildren();
            for (size_t i = 1; i < children.size(); ++i)
            {
                tgNodes n0 = children[i-1]->getNodes();
                tgNodes n1 = children[i  ]->getNodes();

				tgNode mid0((n0[0] + n1[0])/2.0);
				tgNode mid1((n0[1] + n1[1])/2.0);
				tgNode mid2((n0[2] + n1[2])/2.0);
				tgNode mid3((n0[3] + n1[3])/2.0);
#if (0)
				if (i == 1)
				{
					btScalar segDist = (n0[0] - n1[0]).length() / 2.0;
					tgNode start0 = n0[0] + btVector3(segDist, 0.0, 0.0);
					tgNode start1 = n0[1] + btVector3(segDist, 0.0, 0.0);
					tgNode start2 = n0[2] + btVector3(segDist, 0.0, 0.0);
					tgNode start3 = n0[3] + btVector3(segDist, 0.0, 0.0);

					snake.addPair(start0, start1, "segment rod");
					snake.addPair(start2, start3, "segment rod");

					snake.addPair(n0[5], start0, "segment muscle front one");
					snake.addPair(n0[5], start2, "segment muscle front two");
					snake.addPair(n0[7], start1, "segment muscle front three");
					snake.addPair(n0[7], start3, "segment muscle front four");

					snake.addPair(n0[8], start0, "segment muscle rear one");
					snake.addPair(n0[8], start1, "segment muscle rear two");
					snake.addPair(n0[9], start2, "segment muscle rear three");
					snake.addPair(n0[9], start3, "segment muscle rear four");

				}
				if (i == children.size() - 1)
				{
					btScalar segDist = (n0[0] - n1[0]).length() / 2.0;
					tgNode end0 = n1[0] - btVector3(segDist, 0.0, 0.0);
					tgNode end1 = n1[1] - btVector3(segDist, 0.0, 0.0);
					tgNode end2 = n1[2] - btVector3(segDist, 0.0, 0.0);
					tgNode end3 = n1[3] - btVector3(segDist, 0.0, 0.0);

					snake.addPair(end0, end1, "segment rod");
					snake.addPair(end2, end3, "segment rod");

					snake.addPair(n1[4], end0, "segment muscle rear one");
					snake.addPair(n1[4], end2, "segment muscle rear two");
					snake.addPair(n1[6], end1, "segment muscle rear three");
					snake.addPair(n1[6], end3, "segment muscle rear four");

					snake.addPair(n1[10], end0, "segment muscle rear one");
					snake.addPair(n1[10], end1, "segment muscle rear two");
					snake.addPair(n1[11], end2, "segment muscle rear three");
					snake.addPair(n1[11], end3, "segment muscle rear four");
				}
#endif
				snake.addPair(mid0, mid1, "segment rod");
				snake.addPair(mid2, mid3, "segment rod");

                snake.addPair(n0[4], mid0, "segment muscle rear one");
                snake.addPair(n0[4], mid2, "segment muscle rear two");
                snake.addPair(n0[6], mid1, "segment muscle rear three");
                snake.addPair(n0[6], mid3, "segment muscle rear four");

                snake.addPair(n1[5], mid0, "segment muscle front one");
                snake.addPair(n1[5], mid2, "segment muscle front two");
                snake.addPair(n1[7], mid1, "segment muscle front three");
                snake.addPair(n1[7], mid3, "segment muscle front four");

                snake.addPair(n0[10], mid0, "segment muscle rear one");
                snake.addPair(n0[10], mid1, "segment muscle rear two");
                snake.addPair(n0[11], mid2, "segment muscle rear three");
                snake.addPair(n0[11], mid3, "segment muscle rear four");

                snake.addPair(n1[8], mid0, "segment muscle rear one");
                snake.addPair(n1[8], mid1, "segment muscle rear two");
                snake.addPair(n1[9], mid2, "segment muscle rear three");
                snake.addPair(n1[9], mid3, "segment muscle rear four");

            }
    }
} // namespace

CaterpillarModel::CaterpillarModel() : tgModel()
{
}

CaterpillarModel::~CaterpillarModel()
{
}

void CaterpillarModel::addNodes(tgStructure& s)
{
    const double half_length = c.rod_length / 2;

    s.addNode(-c.rod_space,  -half_length, 0);            // 0
    s.addNode(-c.rod_space,   half_length, 0);            // 1
    s.addNode( c.rod_space,  -half_length, 0);            // 2
    s.addNode( c.rod_space,   half_length, 0);            // 3
    s.addNode(0,           -c.rod_space,   -half_length); // 4
    s.addNode(0,           -c.rod_space,    half_length); // 5
    s.addNode(0,            c.rod_space,   -half_length); // 6
    s.addNode(0,            c.rod_space,    half_length); // 7
    s.addNode(-half_length, 0,            c.rod_space);   // 8
    s.addNode( half_length, 0,            c.rod_space);   // 9
    s.addNode(-half_length, 0,           -c.rod_space);   // 10
    s.addNode( half_length, 0,           -c.rod_space);   // 11
}

void CaterpillarModel::addRods(tgStructure& s)
{
    s.addPair( 0,  1, "y rod");
    s.addPair( 2,  3, "y rod");
    s.addPair( 4,  5, "z rod");
    s.addPair( 6,  7, "z rod");
    s.addPair( 8,  9, "x rod");
    s.addPair(10, 11, "x rod");
}

void CaterpillarModel::addMuscles(tgStructure& s)
{
    s.addPair(0, 4,  "muscle");
    s.addPair(0, 5,  "muscle");
    s.addPair(0, 8,  "muscle");
    s.addPair(0, 10, "muscle");

    s.addPair(1, 6,  "muscle");
    s.addPair(1, 7,  "muscle");
    s.addPair(1, 8,  "muscle");
    s.addPair(1, 10, "muscle");

    s.addPair(2, 4,  "muscle");
    s.addPair(2, 5,  "muscle");
    s.addPair(2, 9,  "muscle");
    s.addPair(2, 11, "muscle");

    s.addPair(3, 7,  "muscle");
    s.addPair(3, 6,  "muscle");
    s.addPair(3, 9,  "muscle");
    s.addPair(3, 11, "muscle");

    s.addPair(4, 2,  "muscle");
    s.addPair(4, 10, "muscle");
    s.addPair(4, 11, "muscle");

    s.addPair(5, 8,  "muscle");
    s.addPair(5, 9,  "muscle");

    s.addPair(6, 10, "muscle");
    s.addPair(6, 11, "muscle");

    s.addPair(7, 8,  "muscle");
    s.addPair(7, 9,  "muscle");

}

void CaterpillarModel::setup(tgWorld& world)
{

    const tgRod::Config rodConfig(c.radius, c.density, c.friction,
				c.rollFriction, c.restitution);

    tgLinearString::Config muscleConfig(c.stiffness, c.damping, c.hist, c.rotation,
					    c.maxTens, c.targetVelocity,
					    c.maxAcc);

    // Start creating the structure
    tgStructure s;
    addNodes(s);
    addRods(s);
    addMuscles(s);

    btVector3 rotationPoint = btVector3(0, 0, 0); // origin
    btVector3 rotationAxis = btVector3(0, 1, 0);  // y-axis
    double rotationAngle = M_PI/2;
    s.addRotation(rotationPoint, rotationAxis, rotationAngle);

    s.move(btVector3(0, 10, 0));

	tgStructure caterpillar;
	addSegments(caterpillar, s, 2.0 * c.rod_length, 5);
	addSegmentMuscles(caterpillar);

    // Create the build spec that uses tags to turn the structure into a real model
    tgBuildSpec spec;
    spec.addBuilder("rod", new tgRodInfo(rodConfig));
    spec.addBuilder("muscle", new tgLinearStringInfo(muscleConfig));

    // Create your structureInfo
    tgStructureInfo structureInfo(caterpillar, spec);

    // Use the structureInfo to build ourselves
    structureInfo.buildInto(*this, world);

    // We could now use tgCast::filter or similar to pull out the
    // models (e.g. muscles) that we want to control.
    allMuscles = tgCast::filter<tgModel, tgLinearString> (getDescendants());

    // call the onSetup methods of all observed things e.g. controllers
    notifySetup();

    // Actually setup the children
    tgModel::setup(world);
}

void CaterpillarModel::step(double dt)
{
    // Precondition
    if (dt <= 0.0)
    {
        throw std::invalid_argument("dt is not positive");
    }
    else
    {
        // Notify observers (controllers) of the step so that they can take action
        notifyStep(dt);
        tgModel::step(dt);  // Step any children
    }
}

void CaterpillarModel::onVisit(tgModelVisitor& r)
{
    tgModel::onVisit(r);
}

const std::vector<tgLinearString*>& CaterpillarModel::getAllMuscles() const
{
    return allMuscles;
}

void CaterpillarModel::teardown()
{
    tgModel::teardown();
}
<|endoftext|>


<commit_before>//
// a21 — Arduino Toolkit.
// Copyright (C) 2016-2017, Aleh Dzenisiuk. http://github.com/aleh/a21
//

#pragma once

#include <arduino.h>

namespace a21 {

/**
 * To wrap a series of EC-11 encoder events, such as "clock-wise rotation by 5 steps".
 */
struct EC11Event {

	enum Type {
    /** No event yet. */
		None = 0,
    /** Clock-wise. */
		StepCW,
    /** Counter-clockwise. */
		StepCCW
	};

	/** Direction of the rotation. */
	Type type;

	/** Number of steps in the same direction. */
	uint8_t count;

	EC11Event() : type(None), count(0) {};
};

/**
 * Helps with reading of EC-11 kind of rotary encoders.
 * Does not depend on concrete pin numbers, so can be fed from an interrupt handler or by simple polling.
 */
class EC11 {

private:

	// Here we store last 4 pin states, as pairs of bits, with the least significant ones
	// representing the most recent state.
	uint8_t _lastPinStates;

	// The latest not yet read event.
	EC11Event _event;

	/** Called internally every time we see a quad of pin states representing a single defined step. */
	void addEvent(EC11Event::Type type) {

		if (_event.type == type) {
			// Similar event, consolidate with the recent one by incrementing the counter,
      // saturating it, if needed, to avoid an overflow.
			if (_event.count != 0xFF) {
				_event.count++;
			}
		} else {
			// New direction, restart the step counter.
			_event.type = type;
			_event.count = 1;
		}
	}

public:

	EC11() : _lastPinStates(0) {}

	/**
	 * Must be called either from an interrupt handler associated with pin changes,
	 * or periodically from a polling loop.
	 * The current states of both pins should be passed here.
	 * Note that we expect that the default state of both pins is HIGH when the encoder is at a stable point.
	 */
	void checkPins(bool pinAState, bool pinBState) {

		uint8_t state = (pinBState << 1) | pinAState;
		if (state != (_lastPinStates & 0x3)) {

			// OK, the state of pins has changed compared to the last known, let's record it.

			_lastPinStates = (_lastPinStates << 2) | state;

			// If we see a sequence of codes corresponding to a single step, then let's record an event.
			if (_lastPinStates == 0x87) {
				addEvent(EC11Event::StepCCW);
			} else if (_lastPinStates == 0x4B) {
				addEvent(EC11Event::StepCW);
			}
		}
	}

	/**
	 * This should be called from a normal event loop periodically to check if there are new events coming
	 * from the encoder. When true is returned, then the EC11Event structure the `e` parameter references
   * is filled with the most recent event (which is then marked as "read" and won't be returned the next time).
	 */
	bool read(EC11Event *e) {

		// checkPins() above might be called from an interrupt handler, so we need to make sure we won't access the event at the same time.
		noInterrupts();

		if (_event.count == 0) {
			// Well, no events yet.
			interrupts();
			return false;
		}

		// OK, got one, let's copy it.
		*e = _event;

		// Let's reset the counter, so only new events will be seen the next time.
		_event.count = 0;

		// We assume this function is always called from a normal interrupt-enabled state, so simply re-enabling them.
		interrupts();

		return true;
	}
};

} // namespace
<commit_msg>EC11: add support for single ADC pin configuration<commit_after>//
// a21 — Arduino Toolkit.
// Copyright (C) 2016-2017, Aleh Dzenisiuk. http://github.com/aleh/a21
//

#pragma once

#include <arduino.h>

namespace a21 {

/** To represent EC-11 encoder rotation events, such as "clock-wise rotation by 5 steps". */
struct EC11Event {

	enum Type {

    /** No event yet. */
		None = 0,

    /** Clock-wise rotation. */
		StepCW,

    /** Counter-clockwise rotation. */
		StepCCW
	};

	/** Direction of the rotation. */
	Type type;

	/** Number of steps in the current direction. */
	uint8_t count;

	EC11Event() : type(None), count(0) {}
};

/**
 * Helps reading EC-11 rotary encoders.
 * Does not depend on pin numbers, so can be fed from an interrupt handler or from a polling loop.
 * A typical setup involves reading two digital input pins with internal pull-ups either from a pin change
 * interrupt handler or periodically and then passing their states to checkPins().
 */
class EC11 {

private:

	// Here we store last 4 pin states, as pairs of bits, with the least significant ones
	// representing the most recent state.
	uint8_t _lastPinStates;

	// The latest not yet read event.
	EC11Event _event;

	/** Called internally every time we see a quad of pin states representing a single defined step. */
	void addEvent(EC11Event::Type type) {

		if (_event.type == type) {
			// Similar event, consolidate with the recent one by incrementing the counter,
      // saturating it, if needed, to avoid an overflow.
			if (_event.count != 0xFF) {
				_event.count++;
			}
		} else {
			// New direction, restart the step counter.
			_event.type = type;
			_event.count = 1;
		}
	}

public:

	EC11() : _lastPinStates(0) {}

  /** Resets the current sequence of events, if there is any. Handy when we know that the most recent events could not be caused by normalpin transitions. */
  void reset() {
    _lastPinStates = 0;
  }

	/**
	 * Must be called either from an interrupt handler associated with pin changes,
	 * or periodically from a polling loop.
	 * The current states of both pins should be passed here.
	 * Note that we expect that the default state of both pins is HIGH when the encoder is at a stable point.
	 */
	void checkPins(bool pinAState, bool pinBState, bool switchPinState = true) {

		uint8_t state = (pinBState << 1) | pinAState;
		if (state != (_lastPinStates & 0x3)) {

			// OK, the state of pins has changed compared to the last known, let's record it.

			_lastPinStates = (_lastPinStates << 2) | state;

			// If we see a sequence of codes corresponding to a single step, then let's record an event.
			if (_lastPinStates == 0x87) {
				addEvent(EC11Event::StepCCW);
			} else if (_lastPinStates == 0x4B) {
				addEvent(EC11Event::StepCW);
			}
		}
	}

	/**
	 * This should be called from a normal event loop periodically to check if there are new events coming
	 * from the encoder. When true is returned, then the EC11Event structure the `e` parameter references
   * is filled with the most recent event (which is then marked as "read" and won't be returned the next time).
	 */
	bool read(EC11Event *e) {

		// checkPins() above might be called from an interrupt handler, so we need to make sure we won't access the event at the same time.
		noInterrupts();

		if (_event.count == 0) {
			// Well, no events yet.
			interrupts();
			return false;
		}

		// OK, got one, let's copy it.
		*e = _event;

		// Let's reset the counter, so only new events will be seen the next time.
		_event.count = 0;

		// We assume this function is always called from a normal interrupt-enabled state, so simply re-enabling them.
		interrupts();

		return true;
	}
};

/** To represent EC11 switch events. */
enum EC11PressEvent {
  EC11PressEventNone,
  EC11PressEventDown,
  EC11PressEventUp
};

/**
 * Allows to use a EC11 with a single pin. Handy when you are out of pins but can afford to do ADC fequently enough.
 *
 * The setup uses 3 resistors here: all are connected to your ADC pin with their one end and to Vcc, pin A and pin B
 * of your encoder with their other ends. Their resistances are passed here as R, RA, and RB correspondingly.
 * The max resolution of your ADC is passed here as well (V00). Note that RA should be greater than RB.
 * The default values are optimized for low current draw and maximum difference in ADC values.
 *
 * Working with this class is very similar to working with EC11: you do ADC on your pin fast enough
 * and call checkValue() instead of checkPins(). You can then call read() less freqiently from your interrupt-enabled
 * loop to check if any event was recorded.
 */
template<
  const uint32_t R = 20000,
  const uint32_t RA = 68000,
  const uint32_t RB = 47000,
  const uint16_t V00 = 0x3FF
>
class OnePinEC11 : protected EC11 {

  typedef OnePinEC11<R, RA, RB, V00> Self;

  static Self& getSelf() {
    static Self s = Self();
    return s;
  }

  static const uint32_t RAB = (RA * RB) / (RA + RB);

  static const uint16_t V10 = V00 * RA / (R + RA);
  static const uint16_t V01 = V00 * RB / (R + RB);
  static const uint16_t V11 = V00 * RAB / (R + RAB);

  bool _lastSwitchState;

  /** The next unread press event. */
  EC11PressEvent _pressEvent;

  void checkSwitchPin(bool switchPinState) {

    if (_lastSwitchState != switchPinState) {

      _lastSwitchState = switchPinState;

      _pressEvent = _lastSwitchState ? EC11PressEventUp : EC11PressEventDown;

      // Disregard the recent chain of transitions, if any, because the switching could cause some fake transitions.
      EC11::reset();
    }
  }

public:

  /** Reads (and "eats") the next encoder switch state change event. */
  EC11PressEvent readPress() {

  		noInterrupts();

      // Read and "eat" the event, so it's not returned the next time.
      EC11PressEvent result = _pressEvent;
      _pressEvent = EC11PressEventNone;

      interrupts();

      return result;
  }

  /** Reads (and "eats") the next encoder rotation event. */
  bool readRotation(EC11Event *e) {
    return EC11::read(e);
  }

  /** Should be called often enough from your ADC conversion code. */
  void checkValue(uint16_t v) {

    if (v >= V11 / 2) {

      if (v >= (V00 + V10) / 2) {
        EC11::checkPins(true, true);
      } else if (v >= (V10 + V01) / 2) {
        EC11::checkPins(false, true);
      } else if (v >= (V01 + V11) / 2) {
        EC11::checkPins(true, false);
      } else {
        EC11::checkPins(false, false);
      }

      checkSwitchPin(true);

    } else {

      checkSwitchPin(false);
    }
  }
};

} // namespace
<|endoftext|>


<commit_before>/*
 * File:   dom.hpp
 * Author: ivan
 *
 * Created on October 8, 2015, 3:15 PM
 */

#pragma once

#include <vector>
#include <memory>
#include <ostream>
#include <map>

#include <utki/Unique.hpp>
#include <utki/Void.hpp>

#include <papki/File.hpp>

#include "config.hpp"

namespace svgdom{

struct Length{
	enum class EUnit{
		UNKNOWN,
		NUMBER,
		PERCENT,
		EM,
		EX,
		PX,
		CM,
		IN,
		PT,
		PC
	};

	real value;
	EUnit unit;

	static Length parse(const std::string& str);

	static Length make(real value, EUnit unit);

	bool isValid()const noexcept{
		return this->unit != EUnit::UNKNOWN;
	}
};

class Renderer;

struct Container;
struct Element;

enum class EStyleProperty{
	UNKNOWN,
	FONT,
	FONT_FAMILY,
	FONT_SIZE,
	FONT_SIZE_ADJUST,
	FONT_STRETCH,
	FONT_STYLE,
	FONT_VARIANT,
	FONT_WEIGHT,
	DIRECTION,
	LETTER_SPACING,
	TEXT_DECORATION,
	UNICODE_BIDI,
	WORD_SPACING,
	CLIP,
	COLOR,
	CURSOR,
	DISPLAY,
	OVERFLOW,
	VISIBILITY,
	CLIP_PATH,
    CLIP_RULE,
    MASK,
    OPACITY,
    ENABLE_BACKGROUND,
    FILTER,
    FLOOD_COLOR,
    FLOOD_OPACITY,
    LIGHTING_COLOR,
    STOP_COLOR,
    STOP_OPACITY,
    POINTER_EVENTS,
    COLOR_INTERPOLATION,
    COLOR_INTERPOLATION_FILTERS,
    COLOR_PROFILE,
    COLOR_RENDERING,
    FILL,
    FILL_OPACITY,
    FILL_RULE,
    IMAGE_RENDERING,
    MARKER,
    MARKER_END,
    MARKER_MID,
    MARKER_START,
    SHAPE_RENDERING,
    STROKE,
    STROKE_DASHARRAY,
    STROKE_DASHOFFSET,
    STROKE_LINECAP,
    STROKE_LINEJOIN,
    STROKE_MITERLIMIT,
    STROKE_OPACITY,
    STROKE_WIDTH,
    TEXT_RENDERING,
    ALIGNMENT_BASELINE,
    BASELINE_SHIFT,
    DOMINANT_BASELINE,
    GLYPH_ORIENTATION_HORIZONTAL,
    GLYPH_ORIENTATION_VERTICAL,
    KERNING,
    TEXT_ANCHOR,
    WRITING_MODE
};

struct Rgb{
	real r, g, b;
};

enum class EStrokeLineCap{
	BUTT,
	ROUND,
	SQUARE
};

struct StylePropertyValue{
	enum class ERule{
		NORMAL,
		NONE, //for color property (e.g. fill, stroke, etc.) means that color is 'none'
		INHERIT, //means that property inheritance was explicitly stated
		URL //means that "str" member holds URL
	} rule = ERule::NORMAL;

	bool isNormal()const noexcept{
		return this->rule == ERule::NORMAL;
	}

	bool isNone()const noexcept{
		return this->rule == ERule::NONE;
	}

	bool isUrl()const noexcept{
		return this->rule == ERule::URL;
	}

	union{
		std::uint32_t color;
		real opacity;
		Length length;
		EStrokeLineCap strokeLineCap;
		Element* url; //used if rule is URL
	};

	std::string str;

	static StylePropertyValue parsePaint(const std::string& str);

	std::string paintToString()const;

	Rgb getRgb()const;
};

struct Element : public utki::Unique{
	Container* parent;

	std::string id;

	virtual ~Element()noexcept{}

	void attribsToStream(std::ostream& s)const;

	virtual void toStream(std::ostream& s, unsigned indent = 0)const = 0;

	std::string toString()const;

	virtual void render(Renderer& renderer)const{}

	const StylePropertyValue* getStyleProperty(EStyleProperty property, bool explicitInherit = false)const;

	virtual Element* findById(const std::string& elementId);
};

struct Container : public Element{
	std::vector<std::unique_ptr<Element>> children;

	void childrenToStream(std::ostream& s, unsigned indent)const;

	void render(Renderer& renderer)const;

	Element* findById(const std::string& elementId) override;
};

struct Referencing{
	Element* ref = nullptr;
	std::string iri;

	void attribsToStream(std::ostream& s)const;
};

struct Transformable{
	struct Transformation{
		enum class EType{
			MATRIX,
			TRANSLATE,
			SCALE,
			ROTATE,
			SKEWX,
			SKEWY
		} type;
		union{
			real a;
			real angle;
		};
		union{
			real b;
			real x;
		};
		union{
			real c;
			real y;
		};
		real d, e, f;
	};
	std::vector<Transformation> transformations;

	void attribsToStream(std::ostream& s)const;

	static decltype(Transformable::transformations) parse(const std::string& str);
};

struct Styleable{
	std::map<EStyleProperty, StylePropertyValue> styles;

	void attribsToStream(std::ostream& s)const;

	static decltype(styles) parse(const std::string& str);

	static std::string propertyToString(EStyleProperty p);
	static EStyleProperty stringToProperty(std::string str);
};

struct GElement : public Container, public Transformable, public Styleable{
	void toStream(std::ostream& s, unsigned indent = 0)const override;

	void render(Renderer& renderer)const override;
};

struct DefsElement : public Container, public Transformable, public Styleable{
	void toStream(std::ostream& s, unsigned indent = 0)const override;
};

struct Rectangle{
	Length x = Length::make(0, Length::EUnit::PERCENT);
	Length y = Length::make(0, Length::EUnit::PERCENT);
	Length width = Length::make(100, Length::EUnit::PERCENT);
	Length height = Length::make(100, Length::EUnit::PERCENT);

	void attribsToStream(std::ostream& s)const;
};

struct SvgElement : public Container, public Rectangle{
	void toStream(std::ostream& s, unsigned indent = 0)const override;

	void render(Renderer& renderer) const override;
};

struct Shape : public Element, public Styleable, public Transformable{
	void attribsToStream(std::ostream& s)const;
};

struct PathElement : public Shape{
	struct Step{
		enum class EType{
			UNKNOWN,
			CLOSE,
			MOVE_ABS,
			MOVE_REL,
			LINE_ABS,
			LINE_REL,
			HORIZONTAL_LINE_ABS,
			HORIZONTAL_LINE_REL,
			VERTICAL_LINE_ABS,
			VERTICAL_LINE_REL,
			CUBIC_ABS,
			CUBIC_REL,
			CUBIC_SMOOTH_ABS,
			CUBIC_SMOOTH_REL,
			QUADRATIC_ABS,
			QUADRATIC_REL,
			QUADRATIC_SMOOTH_ABS,
			QUADRATIC_SMOOTH_REL,
			ARC_ABS,
			ARC_REL
		} type;

		real x, y;

		union{
			real x1;
			real rx;
		};

		union{
			real y1;
			real ry;
		};

		union{
			real x2;
			real xAxisRotation;
		};

		union{
			real y2;
			struct{
				bool largeArc;
				bool sweep;
			} flags;
		};

		static EType charToType(char c);
		static char typeToChar(EType t);
	};
	std::vector<Step> path;

	void attribsToStream(std::ostream& s)const;

	void toStream(std::ostream& s, unsigned indent = 0)const override;

	static decltype(path) parse(const std::string& str);

	void render(Renderer& renderer) const override;
};

struct RectElement : public Shape, public Rectangle{
	Length rx = Length::make(0, Length::EUnit::UNKNOWN);
	Length ry = Length::make(0, Length::EUnit::UNKNOWN);

	void attribsToStream(std::ostream& s)const;

	void toStream(std::ostream& s, unsigned indent) const override;

	void render(Renderer& renderer) const override;
};

struct EllipseElement : public Shape{
	Length cx = Length::make(0, Length::EUnit::UNKNOWN);
	Length cy = Length::make(0, Length::EUnit::UNKNOWN);
	Length rx = Length::make(0, Length::EUnit::UNKNOWN);
	Length ry = Length::make(0, Length::EUnit::UNKNOWN);

	void attribsToStream(std::ostream& s)const;

	void toStream(std::ostream& s, unsigned indent) const override;

	void render(Renderer& renderer) const override;
};

struct Gradient : public Container, public Referencing, public Styleable{
	enum class ESpreadMethod{
		DEFAULT,
		PAD,
		REFLECT,
		REPEAT
	} spreadMethod = ESpreadMethod::DEFAULT;

	static std::string spreadMethodToString(ESpreadMethod sm);
	static ESpreadMethod stringToSpreadMethod(const std::string& str);

	ESpreadMethod getSpreadMethod()const noexcept;

	struct StopElement : public Styleable, public Element{
		real offset;

		void toStream(std::ostream& s, unsigned indent)const override;
	};

	const decltype(Container::children)& getStops()const noexcept;

	void attribsToStream(std::ostream& s)const;
};

struct LinearGradientElement : public Gradient{
	Length x1 = Length::make(0, Length::EUnit::UNKNOWN);
	Length y1 = Length::make(0, Length::EUnit::UNKNOWN);
	Length x2 = Length::make(100, Length::EUnit::UNKNOWN);
	Length y2 = Length::make(0, Length::EUnit::UNKNOWN);

	Length getX1()const noexcept;
	Length getY1()const noexcept;
	Length getX2()const noexcept;
	Length getY2()const noexcept;

	void toStream(std::ostream& s, unsigned indent) const override;
};

struct RadialGradientElement : public Gradient{
	Length cx = Length::make(50, Length::EUnit::UNKNOWN);
	Length cy = Length::make(50, Length::EUnit::UNKNOWN);
	Length r = Length::make(50, Length::EUnit::UNKNOWN);
	Length fx = Length::make(50, Length::EUnit::UNKNOWN);
	Length fy = Length::make(50, Length::EUnit::UNKNOWN);

	Length getCx()const noexcept;
	Length getCy()const noexcept;
	Length getR()const noexcept;

	//can return unit as UNKNOWN in that case fx should coincide with cx
	Length getFx()const noexcept;

	//can return unit as UNKNOWN in that case fy should coincide with cy
	Length getFy()const noexcept;

	void toStream(std::ostream& s, unsigned indent) const override;
};

class Renderer{
public:
	virtual void render(const PathElement& e){}
	virtual void render(const RectElement& e){}
	virtual void render(const EllipseElement& e){}

	virtual void render(const GElement& e){}

	virtual void render(const SvgElement& e){}

	virtual ~Renderer()noexcept{}
};

std::unique_ptr<SvgElement> load(const papki::File& f);

}//~namespace

std::ostream& operator<<(std::ostream& s, const svgdom::Length& l);
<commit_msg>stuff<commit_after>/*
 * File:   dom.hpp
 * Author: ivan
 *
 * Created on October 8, 2015, 3:15 PM
 */

#pragma once

#include <vector>
#include <memory>
#include <ostream>
#include <map>

#include <utki/Unique.hpp>
#include <utki/Void.hpp>

#include <papki/File.hpp>

#include "config.hpp"

namespace svgdom{

struct Length{
	enum class EUnit{
		UNKNOWN,
		NUMBER,
		PERCENT,
		EM,
		EX,
		PX,
		CM,
		IN,
		PT,
		PC
	};

	real value;
	EUnit unit;

	static Length parse(const std::string& str);

	static Length make(real value, EUnit unit);

	bool isValid()const noexcept{
		return this->unit != EUnit::UNKNOWN;
	}

	bool isPercent()const noexcept{
		return this->unit == EUnit::PERCENT;
	}
};

class Renderer;

struct Container;
struct Element;

enum class EStyleProperty{
	UNKNOWN,
	FONT,
	FONT_FAMILY,
	FONT_SIZE,
	FONT_SIZE_ADJUST,
	FONT_STRETCH,
	FONT_STYLE,
	FONT_VARIANT,
	FONT_WEIGHT,
	DIRECTION,
	LETTER_SPACING,
	TEXT_DECORATION,
	UNICODE_BIDI,
	WORD_SPACING,
	CLIP,
	COLOR,
	CURSOR,
	DISPLAY,
	OVERFLOW,
	VISIBILITY,
	CLIP_PATH,
    CLIP_RULE,
    MASK,
    OPACITY,
    ENABLE_BACKGROUND,
    FILTER,
    FLOOD_COLOR,
    FLOOD_OPACITY,
    LIGHTING_COLOR,
    STOP_COLOR,
    STOP_OPACITY,
    POINTER_EVENTS,
    COLOR_INTERPOLATION,
    COLOR_INTERPOLATION_FILTERS,
    COLOR_PROFILE,
    COLOR_RENDERING,
    FILL,
    FILL_OPACITY,
    FILL_RULE,
    IMAGE_RENDERING,
    MARKER,
    MARKER_END,
    MARKER_MID,
    MARKER_START,
    SHAPE_RENDERING,
    STROKE,
    STROKE_DASHARRAY,
    STROKE_DASHOFFSET,
    STROKE_LINECAP,
    STROKE_LINEJOIN,
    STROKE_MITERLIMIT,
    STROKE_OPACITY,
    STROKE_WIDTH,
    TEXT_RENDERING,
    ALIGNMENT_BASELINE,
    BASELINE_SHIFT,
    DOMINANT_BASELINE,
    GLYPH_ORIENTATION_HORIZONTAL,
    GLYPH_ORIENTATION_VERTICAL,
    KERNING,
    TEXT_ANCHOR,
    WRITING_MODE
};

struct Rgb{
	real r, g, b;
};

enum class EStrokeLineCap{
	BUTT,
	ROUND,
	SQUARE
};

struct StylePropertyValue{
	enum class ERule{
		NORMAL,
		NONE, //for color property (e.g. fill, stroke, etc.) means that color is 'none'
		INHERIT, //means that property inheritance was explicitly stated
		URL //means that "str" member holds URL
	} rule = ERule::NORMAL;

	bool isNormal()const noexcept{
		return this->rule == ERule::NORMAL;
	}

	bool isNone()const noexcept{
		return this->rule == ERule::NONE;
	}

	bool isUrl()const noexcept{
		return this->rule == ERule::URL;
	}

	union{
		std::uint32_t color;
		real opacity;
		Length length;
		EStrokeLineCap strokeLineCap;
		Element* url; //used if rule is URL
	};

	std::string str;

	static StylePropertyValue parsePaint(const std::string& str);

	std::string paintToString()const;

	Rgb getRgb()const;
};

struct Element : public utki::Unique{
	Container* parent;

	std::string id;

	virtual ~Element()noexcept{}

	void attribsToStream(std::ostream& s)const;

	virtual void toStream(std::ostream& s, unsigned indent = 0)const = 0;

	std::string toString()const;

	virtual void render(Renderer& renderer)const{}

	const StylePropertyValue* getStyleProperty(EStyleProperty property, bool explicitInherit = false)const;

	virtual Element* findById(const std::string& elementId);
};

struct Container : public Element{
	std::vector<std::unique_ptr<Element>> children;

	void childrenToStream(std::ostream& s, unsigned indent)const;

	void render(Renderer& renderer)const;

	Element* findById(const std::string& elementId) override;
};

struct Referencing{
	Element* ref = nullptr;
	std::string iri;

	void attribsToStream(std::ostream& s)const;
};

struct Transformable{
	struct Transformation{
		enum class EType{
			MATRIX,
			TRANSLATE,
			SCALE,
			ROTATE,
			SKEWX,
			SKEWY
		} type;
		union{
			real a;
			real angle;
		};
		union{
			real b;
			real x;
		};
		union{
			real c;
			real y;
		};
		real d, e, f;
	};
	std::vector<Transformation> transformations;

	void attribsToStream(std::ostream& s)const;

	static decltype(Transformable::transformations) parse(const std::string& str);
};

struct Styleable{
	std::map<EStyleProperty, StylePropertyValue> styles;

	void attribsToStream(std::ostream& s)const;

	static decltype(styles) parse(const std::string& str);

	static std::string propertyToString(EStyleProperty p);
	static EStyleProperty stringToProperty(std::string str);
};

struct GElement : public Container, public Transformable, public Styleable{
	void toStream(std::ostream& s, unsigned indent = 0)const override;

	void render(Renderer& renderer)const override;
};

struct DefsElement : public Container, public Transformable, public Styleable{
	void toStream(std::ostream& s, unsigned indent = 0)const override;
};

struct Rectangle{
	Length x = Length::make(0, Length::EUnit::PERCENT);
	Length y = Length::make(0, Length::EUnit::PERCENT);
	Length width = Length::make(100, Length::EUnit::PERCENT);
	Length height = Length::make(100, Length::EUnit::PERCENT);

	void attribsToStream(std::ostream& s)const;
};

struct SvgElement : public Container, public Rectangle{
	void toStream(std::ostream& s, unsigned indent = 0)const override;

	void render(Renderer& renderer) const override;
};

struct Shape : public Element, public Styleable, public Transformable{
	void attribsToStream(std::ostream& s)const;
};

struct PathElement : public Shape{
	struct Step{
		enum class EType{
			UNKNOWN,
			CLOSE,
			MOVE_ABS,
			MOVE_REL,
			LINE_ABS,
			LINE_REL,
			HORIZONTAL_LINE_ABS,
			HORIZONTAL_LINE_REL,
			VERTICAL_LINE_ABS,
			VERTICAL_LINE_REL,
			CUBIC_ABS,
			CUBIC_REL,
			CUBIC_SMOOTH_ABS,
			CUBIC_SMOOTH_REL,
			QUADRATIC_ABS,
			QUADRATIC_REL,
			QUADRATIC_SMOOTH_ABS,
			QUADRATIC_SMOOTH_REL,
			ARC_ABS,
			ARC_REL
		} type;

		real x, y;

		union{
			real x1;
			real rx;
		};

		union{
			real y1;
			real ry;
		};

		union{
			real x2;
			real xAxisRotation;
		};

		union{
			real y2;
			struct{
				bool largeArc;
				bool sweep;
			} flags;
		};

		static EType charToType(char c);
		static char typeToChar(EType t);
	};
	std::vector<Step> path;

	void attribsToStream(std::ostream& s)const;

	void toStream(std::ostream& s, unsigned indent = 0)const override;

	static decltype(path) parse(const std::string& str);

	void render(Renderer& renderer) const override;
};

struct RectElement : public Shape, public Rectangle{
	Length rx = Length::make(0, Length::EUnit::UNKNOWN);
	Length ry = Length::make(0, Length::EUnit::UNKNOWN);

	void attribsToStream(std::ostream& s)const;

	void toStream(std::ostream& s, unsigned indent) const override;

	void render(Renderer& renderer) const override;
};

struct EllipseElement : public Shape{
	Length cx = Length::make(0, Length::EUnit::UNKNOWN);
	Length cy = Length::make(0, Length::EUnit::UNKNOWN);
	Length rx = Length::make(0, Length::EUnit::UNKNOWN);
	Length ry = Length::make(0, Length::EUnit::UNKNOWN);

	void attribsToStream(std::ostream& s)const;

	void toStream(std::ostream& s, unsigned indent) const override;

	void render(Renderer& renderer) const override;
};

struct Gradient : public Container, public Referencing, public Styleable{
	enum class ESpreadMethod{
		DEFAULT,
		PAD,
		REFLECT,
		REPEAT
	} spreadMethod = ESpreadMethod::DEFAULT;

	static std::string spreadMethodToString(ESpreadMethod sm);
	static ESpreadMethod stringToSpreadMethod(const std::string& str);

	ESpreadMethod getSpreadMethod()const noexcept;

	struct StopElement : public Styleable, public Element{
		real offset;

		void toStream(std::ostream& s, unsigned indent)const override;
	};

	const decltype(Container::children)& getStops()const noexcept;

	void attribsToStream(std::ostream& s)const;
};

struct LinearGradientElement : public Gradient{
	Length x1 = Length::make(0, Length::EUnit::UNKNOWN);
	Length y1 = Length::make(0, Length::EUnit::UNKNOWN);
	Length x2 = Length::make(100, Length::EUnit::UNKNOWN);
	Length y2 = Length::make(0, Length::EUnit::UNKNOWN);

	Length getX1()const noexcept;
	Length getY1()const noexcept;
	Length getX2()const noexcept;
	Length getY2()const noexcept;

	void toStream(std::ostream& s, unsigned indent) const override;
};

struct RadialGradientElement : public Gradient{
	Length cx = Length::make(50, Length::EUnit::UNKNOWN);
	Length cy = Length::make(50, Length::EUnit::UNKNOWN);
	Length r = Length::make(50, Length::EUnit::UNKNOWN);
	Length fx = Length::make(50, Length::EUnit::UNKNOWN);
	Length fy = Length::make(50, Length::EUnit::UNKNOWN);

	Length getCx()const noexcept;
	Length getCy()const noexcept;
	Length getR()const noexcept;

	//can return unit as UNKNOWN in that case fx should coincide with cx
	Length getFx()const noexcept;

	//can return unit as UNKNOWN in that case fy should coincide with cy
	Length getFy()const noexcept;

	void toStream(std::ostream& s, unsigned indent) const override;
};

class Renderer{
public:
	virtual void render(const PathElement& e){}
	virtual void render(const RectElement& e){}
	virtual void render(const EllipseElement& e){}

	virtual void render(const GElement& e){}

	virtual void render(const SvgElement& e){}

	virtual ~Renderer()noexcept{}
};

std::unique_ptr<SvgElement> load(const papki::File& f);

}//~namespace

std::ostream& operator<<(std::ostream& s, const svgdom::Length& l);
<|endoftext|>


<commit_before>/**************************************************************************
 * The MIT License                                                        *
 * Copyright (c) 2013 Antony Arciuolo.                                    *
 * arciuolo@gmail.com                                                     *
 *                                                                        *
 * Permission is hereby granted, free of charge, to any person obtaining  *
 * a copy of this software and associated documentation files (the        *
 * "Software"), to deal in the Software without restriction, including    *
 * without limitation the rights to use, copy, modify, merge, publish,    *
 * distribute, sublicense, and/or sell copies of the Software, and to     *
 * permit persons to whom the Software is furnished to do so, subject to  *
 * the following conditions:                                              *
 *                                                                        *
 * The above copyright notice and this permission notice shall be         *
 * included in all copies or substantial portions of the Software.        *
 *                                                                        *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        *
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     *
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND                  *
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE *
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION *
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION  *
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.        *
 **************************************************************************/
#include <oSurface/codec.h>
#include <oSurface/convert.h>
#include <dxgi.h>
#include "dds.h"

namespace ouro { namespace surface {

static surface::format from_dds_format(DDS_FORMAT format)
{
	switch (format)
	{
		case DDS_FORMAT_R8G8B8_UNORM: return surface::format::r8g8b8_unorm;
		case DDS_FORMAT_B8G8R8_UNORM: return surface::format::b8g8r8_unorm;
		default: break;
	}

	return (surface::format)format;
}

static DDS_FORMAT to_dds_format(surface::format f)
{
	return (DDS_FORMAT)f;
}

dds_pixel_format GetPixelFormat(const surface::format& f)
{
	static const dds_pixel_format bgr = DDSPF_B8G8R8_UNORM;
	static const dds_pixel_format rgb = DDSPF_R8G8B8_UNORM;

	switch (f)
	{
		case format::b8g8r8_unorm: return bgr;
		case format::r8g8b8_unorm: return rgb;
		default: break;
	}
	return to_ddspf(to_dds_format(f));
}

bool is_dds(const void* buffer, size_t size)
{
	return size >= (sizeof(uint32_t) + sizeof(dds_header)) && *(const uint32_t*)buffer == dds_signature;
}

info get_info_dds(const void* buffer, size_t size)
{
	if (!is_dds(buffer, size))
		return info();

	auto* h = (const dds_header*)byte_add(buffer, sizeof(uint32_t));
  if (h->dwSize != sizeof(dds_header) || h->ddspf.dwSize != sizeof(dds_pixel_format))
		return info();

	const bool kIsD3D10 = has_dx10_header(*h);

	if ((kIsD3D10 && size < (sizeof(dds_header) + sizeof(uint32_t) + sizeof(dds_header_dx10))))
		return info();

	bool cubemap = false;
	DDS_RESOURCE_DIMENSION resourceType;

	info si;
	si.dimensions = int3(h->dwWidth, h->dwHeight, h->dwDepth);
	si.mip_layout = (h->dwFlags & DDS_HEADER_FLAGS_MIPMAP) && h->dwMipMapCount != 1 ? mip_layout::tight : mip_layout::none;

	if (kIsD3D10)
	{
		auto d3d10ext = (const dds_header_dx10*)&h[1];
		si.array_size = d3d10ext->arraySize;
		if (!si.array_size) // invalid according to DDS
			return info();

		switch (d3d10ext->dxgiFormat)
		{
			case DDS_FORMAT_AI44:
			case DDS_FORMAT_IA44:
			case DDS_FORMAT_P8:
			case DDS_FORMAT_A8P8: return info(); // not supported
			default:
				if (bits(from_dds_format(d3d10ext->dxgiFormat)) == 0)
					return info();
		}

		si.format = from_dds_format(d3d10ext->dxgiFormat);

		switch (d3d10ext->resourceDimension)
		{
			case DDS_RESOURCE_DIMENSION_TEXTURE1D:
				// D3DX writes 1D textures with a fixed Height of 1
				if ((h->dwFlags & DDS_HEIGHT) && h->dwHeight != 1)
					return info();
				si.dimensions.y = si.dimensions.z = 1;
				break;

			case DDS_RESOURCE_DIMENSION_TEXTURE2D:
				if (d3d10ext->miscFlag & DDS_RESOURCE_MISC_TEXTURECUBE)
				{
					si.array_size *= 6;
					cubemap = true;
				}
				si.dimensions.z = 1;
				break;

			case DDS_RESOURCE_DIMENSION_TEXTURE3D:
				if (!(h->dwFlags & DDS_HEADER_FLAGS_VOLUME))
					return info(); // ERROR_INVALID_DATA
				if (si.array_size > 1)
					return info(); // ERROR_NOT_SUPPORTED
				break;

			default:
				return info();
		}

		resourceType = d3d10ext->resourceDimension;
	}

	else
	{
		si.format = from_dds_format(from_ddspf(h->ddspf));
		if (si.format == format::unknown)
		{
			if (h->ddspf.dwRGBBitCount == 24)
			{
				auto ddpf = h->ddspf;
				if (ISBITMASK(0x000000ff,0x0000ff00,0x00ff0000,0x00000000)) si.format = format::r8g8b8_unorm;
				else if (ISBITMASK(0x00ff0000,0x0000ff00,0x000000ff,0x00000000)) si.format = format::b8g8r8_unorm;
				else return info();
			}
			else
				return info();
		}

		if (h->dwFlags & DDS_HEADER_FLAGS_VOLUME)
			resourceType = DDS_RESOURCE_DIMENSION_TEXTURE3D;
		else
		{
			if (h->dwCaps2 & DDS_CUBEMAP)
			{
				// We require all six faces to be defined
				if ((h->dwCaps2 & DDS_CUBEMAP_ALLFACES ) != DDS_CUBEMAP_ALLFACES)
					return info();
				si.array_size = 6;
				cubemap = true;
			}

			si.dimensions.z = 1;
			resourceType = DDS_RESOURCE_DIMENSION_TEXTURE2D;
			// No way for a legacy Direct3D 9 DDS to express a 1D texture
		}

		oCHECK0(bits(si.format));
	}

	// There's no way to distinguish between a not-array and an array of size 1.
	// To be consistent with other formats rather than considering every image
	// an array of size 1, consider any array of size 1 not an array.
	if (si.array_size == 1)
		si.array_size = 0;

	return si;
}

static void map_bits(const info& inf, const void* oRESTRICT src_dds_buffer, size_t src_dds_size, const_mapped_subresource* oRESTRICT subresources, size_t num_subresources)
{
	const void* end = byte_add(src_dds_buffer, src_dds_size);
	const uint nMips = max(1u, num_mips(inf.mip_layout != mip_layout::none, inf.dimensions));
	const uint nSlices = max(1u, inf.array_size);

	size_t index = 0;
	for (uint j = 0; j < nSlices; j++)
	{
		for (uint i = 0; i < nMips; i++ )
		{
			uint subresource = calc_subresource(i, j, 0, nMips, nSlices);
			subresources[subresource] = get_const_mapped_subresource(inf, subresource, 0, src_dds_buffer);
			oCHECK(byte_add(subresources[subresource].data, subresources[subresource].depth_pitch) <= end, "end of file");
		}
	}
}

static void map_bits(const info& inf, const void* oRESTRICT src_dds_buffer, size_t src_dds_size, mapped_subresource* oRESTRICT subresources, size_t num_subresources)
{
	map_bits(inf, src_dds_buffer, src_dds_size, (const_mapped_subresource*)subresources, num_subresources);
}

scoped_allocation encode_dds(const texel_buffer& b, const alpha_option& option, const compression& compression)
{
	//if (1) oTHROW(operation_not_supported, "dds not yet implemented");

	const auto inf = b.get_info();
	oCHECK(inf.mip_layout == mip_layout::none || inf.mip_layout == mip_layout::tight, "right and below mip layouts not supported");

	const bool is3d = false; // how to specify? probably .z != 0...
	const bool isbc = dds_is_block_compressed(to_dds_format(inf.format));
	const bool mips = inf.mip_layout != mip_layout::none;
	const bool iscube = false; // how to specify?

	const dds_pixel_format ddspf = GetPixelFormat(inf.format);
	const bool kHasDX10 = iscube || inf.array_size > 1 || ddspf.dwFourCC == MAKEFOURCC('D','X','1','0');

	const size_t bits_size = b.size();
	const size_t size = bits_size + sizeof(dds_signature) + sizeof(dds_header)
		+ (kHasDX10 ? sizeof(dds_header_dx10) : 0);

	scoped_allocation a(default_allocate(size, 0), size, default_deallocate);

	*(uint*)a = dds_signature;
	auto h = (dds_header*)byte_add((void*)a, sizeof(dds_signature));
	auto d3d10ext = (dds_header_dx10*)&h[1];
	void* bits = kHasDX10 ? (void*)&d3d10ext[1] : (void*)&h[1];

	uint pitch = dds_calc_pitch(to_dds_format(inf.format), inf.dimensions.x);
	h->dwSize = sizeof(dds_header);
	h->dwFlags = DDS_HEADER_FLAGS_TEXTURE | (mips ? DDS_HEADER_FLAGS_MIPMAP : 0) | (isbc ? DDS_HEADER_FLAGS_LINEARSIZE : 0) | (is3d ? DDS_HEADER_FLAGS_VOLUME : 0);
	h->dwHeight = inf.dimensions.y;
	h->dwWidth = inf.dimensions.x;
	h->dwPitchOrLinearSize = isbc ? pitch : 0;
	h->dwDepth = is3d ? inf.dimensions.z : 0;
	h->dwMipMapCount = mips ? num_mips(inf) : 0;
	memset(h->dwReserved1, 0, sizeof(h->dwReserved1));
	h->ddspf = ddspf;
	h->dwCaps = DDS_SURFACE_FLAGS_TEXTURE | ((is3d || mips || inf.array_size) ? DDS_SURFACE_FLAGS_COMPLEX : 0) | (mips ? DDS_SURFACE_FLAGS_MIPMAP : 0);
	h->dwCaps2 = (iscube ? DDS_CUBEMAP_ALLFACES : 0) | (is3d ? DDS_FLAGS_VOLUME : 0);
	h->dwCaps3 = 0;
	h->dwCaps4 = 0;
	h->dwReserved2 = 0;
	if (kHasDX10)
	{
		d3d10ext->dxgiFormat = to_dds_format(inf.format);
		d3d10ext->resourceDimension = is3d ? DDS_RESOURCE_DIMENSION_TEXTURE3D : DDS_RESOURCE_DIMENSION_TEXTURE2D; // how to tell 1D? y==0 && z==0?
		d3d10ext->miscFlag = iscube ? DDS_RESOURCE_MISC_TEXTURECUBE : 0;
		d3d10ext->arraySize = is3d ? 1u : max(1u, inf.array_size); // DDS requires this to be non-zero
		d3d10ext->miscFlags2 = 0;
	}

	const int nSubresources = num_subresources(inf);
	mapped_subresource* subresources = (mapped_subresource*)default_allocate(sizeof(mapped_subresource) * nSubresources, 0);
	finally DeleteInitData([&] { if (subresources) default_deallocate(subresources); });
	map_bits(inf, bits, bits_size, subresources, nSubresources);

	for (int i = 0; i < nSubresources; i++)
		b.copy_to(i, subresources[i]);

	return a;
}

texel_buffer decode_dds(const void* buffer, size_t size, const alpha_option& option, const mip_layout& layout)
{
	//oCHECK(option == alpha_option::preserve, "changing alpha option not supported for dds");
	info inf = get_info_dds(buffer, size);
	oCHECK(inf.format != format::unknown, "invalid dds");
	texel_buffer b(inf);

	const uint nSubresources = num_subresources(inf);
	const_mapped_subresource* subresources = (const_mapped_subresource*)default_allocate(sizeof(const_mapped_subresource) * nSubresources, 0);
	finally DeleteInitData([&] { if (subresources) default_deallocate(subresources); });

	auto h = (const dds_header*)byte_add(buffer, sizeof(dds_header));
	const void* bits = byte_add(buffer, sizeof(dds_signature) + sizeof(dds_header) + (has_dx10_header(*h) ? sizeof(dds_header_dx10) : 0));
	map_bits(inf, bits, size - byte_diff(bits, buffer), subresources, nSubresources);

	for (uint i = 0; i < nSubresources; i++)
		b.update_subresource(i, subresources[i]);

	return b;
}

}}
<commit_msg>Fix DX10-compatible format decode<commit_after>/**************************************************************************
 * The MIT License                                                        *
 * Copyright (c) 2013 Antony Arciuolo.                                    *
 * arciuolo@gmail.com                                                     *
 *                                                                        *
 * Permission is hereby granted, free of charge, to any person obtaining  *
 * a copy of this software and associated documentation files (the        *
 * "Software"), to deal in the Software without restriction, including    *
 * without limitation the rights to use, copy, modify, merge, publish,    *
 * distribute, sublicense, and/or sell copies of the Software, and to     *
 * permit persons to whom the Software is furnished to do so, subject to  *
 * the following conditions:                                              *
 *                                                                        *
 * The above copyright notice and this permission notice shall be         *
 * included in all copies or substantial portions of the Software.        *
 *                                                                        *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,        *
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF     *
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND                  *
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE *
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION *
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION  *
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.        *
 **************************************************************************/
#include <oSurface/codec.h>
#include <oSurface/convert.h>
#include <dxgi.h>
#include "dds.h"

namespace ouro { namespace surface {

static surface::format from_dds_format(DDS_FORMAT format)
{
	switch (format)
	{
		case DDS_FORMAT_R8G8B8_UNORM: return surface::format::r8g8b8_unorm;
		case DDS_FORMAT_B8G8R8_UNORM: return surface::format::b8g8r8_unorm;
		default: break;
	}

	return (surface::format)format;
}

static DDS_FORMAT to_dds_format(surface::format f)
{
	return (DDS_FORMAT)f;
}

dds_pixel_format GetPixelFormat(const surface::format& f)
{
	static const dds_pixel_format bgr = DDSPF_B8G8R8_UNORM;
	static const dds_pixel_format rgb = DDSPF_R8G8B8_UNORM;

	switch (f)
	{
		case format::b8g8r8_unorm: return bgr;
		case format::r8g8b8_unorm: return rgb;
		default: break;
	}
	return to_ddspf(to_dds_format(f));
}

bool is_dds(const void* buffer, size_t size)
{
	return size >= (sizeof(uint32_t) + sizeof(dds_header)) && *(const uint32_t*)buffer == dds_signature;
}

info get_info_dds(const void* buffer, size_t size)
{
	if (!is_dds(buffer, size))
		return info();

	auto* h = (const dds_header*)byte_add(buffer, sizeof(uint32_t));
  if (h->dwSize != sizeof(dds_header) || h->ddspf.dwSize != sizeof(dds_pixel_format))
		return info();

	const bool kIsD3D10 = has_dx10_header(*h);

	if ((kIsD3D10 && size < (sizeof(dds_header) + sizeof(uint32_t) + sizeof(dds_header_dx10))))
		return info();

	bool cubemap = false;
	DDS_RESOURCE_DIMENSION resourceType;

	info si;
	si.dimensions = int3(h->dwWidth, h->dwHeight, h->dwDepth);
	si.mip_layout = (h->dwFlags & DDS_HEADER_FLAGS_MIPMAP) && h->dwMipMapCount != 1 ? mip_layout::tight : mip_layout::none;

	if (kIsD3D10)
	{
		auto d3d10ext = (const dds_header_dx10*)&h[1];
		si.array_size = d3d10ext->arraySize;
		if (!si.array_size) // invalid according to DDS
			return info();

		switch (d3d10ext->dxgiFormat)
		{
			case DDS_FORMAT_AI44:
			case DDS_FORMAT_IA44:
			case DDS_FORMAT_P8:
			case DDS_FORMAT_A8P8: return info(); // not supported
			default:
				if (bits(from_dds_format(d3d10ext->dxgiFormat)) == 0)
					return info();
		}

		si.format = from_dds_format(d3d10ext->dxgiFormat);

		switch (d3d10ext->resourceDimension)
		{
			case DDS_RESOURCE_DIMENSION_TEXTURE1D:
				// D3DX writes 1D textures with a fixed Height of 1
				if ((h->dwFlags & DDS_HEIGHT) && h->dwHeight != 1)
					return info();
				si.dimensions.y = si.dimensions.z = 1;
				break;

			case DDS_RESOURCE_DIMENSION_TEXTURE2D:
				if (d3d10ext->miscFlag & DDS_RESOURCE_MISC_TEXTURECUBE)
				{
					si.array_size *= 6;
					cubemap = true;
				}
				si.dimensions.z = 1;
				break;

			case DDS_RESOURCE_DIMENSION_TEXTURE3D:
				if (!(h->dwFlags & DDS_HEADER_FLAGS_VOLUME))
					return info(); // ERROR_INVALID_DATA
				if (si.array_size > 1)
					return info(); // ERROR_NOT_SUPPORTED
				break;

			default:
				return info();
		}

		resourceType = d3d10ext->resourceDimension;
	}

	else
	{
		si.format = from_dds_format(from_ddspf(h->ddspf));
		if (si.format == format::unknown)
		{
			if (h->ddspf.dwRGBBitCount == 24)
			{
				auto ddpf = h->ddspf;
				if (ISBITMASK(0x000000ff,0x0000ff00,0x00ff0000,0x00000000)) si.format = format::r8g8b8_unorm;
				else if (ISBITMASK(0x00ff0000,0x0000ff00,0x000000ff,0x00000000)) si.format = format::b8g8r8_unorm;
				else return info();
			}
			else
				return info();
		}

		if (h->dwFlags & DDS_HEADER_FLAGS_VOLUME)
			resourceType = DDS_RESOURCE_DIMENSION_TEXTURE3D;
		else
		{
			if (h->dwCaps2 & DDS_CUBEMAP)
			{
				// We require all six faces to be defined
				if ((h->dwCaps2 & DDS_CUBEMAP_ALLFACES ) != DDS_CUBEMAP_ALLFACES)
					return info();
				si.array_size = 6;
				cubemap = true;
			}

			si.dimensions.z = 1;
			resourceType = DDS_RESOURCE_DIMENSION_TEXTURE2D;
			// No way for a legacy Direct3D 9 DDS to express a 1D texture
		}

		oCHECK0(bits(si.format));
	}

	// There's no way to distinguish between a not-array and an array of size 1.
	// To be consistent with other formats rather than considering every image
	// an array of size 1, consider any array of size 1 not an array.
	if (si.array_size == 1)
		si.array_size = 0;

	return si;
}

static void map_bits(const info& inf, const void* oRESTRICT src_dds_buffer, size_t src_dds_size, const_mapped_subresource* oRESTRICT subresources, size_t num_subresources)
{
	const void* end = byte_add(src_dds_buffer, src_dds_size);
	const uint nMips = max(1u, num_mips(inf.mip_layout != mip_layout::none, inf.dimensions));
	const uint nSlices = max(1u, inf.array_size);

	size_t index = 0;
	for (uint j = 0; j < nSlices; j++)
	{
		for (uint i = 0; i < nMips; i++ )
		{
			uint subresource = calc_subresource(i, j, 0, nMips, nSlices);
			subresources[subresource] = get_const_mapped_subresource(inf, subresource, 0, src_dds_buffer);
			oCHECK(byte_add(subresources[subresource].data, subresources[subresource].depth_pitch) <= end, "end of file");
		}
	}
}

static void map_bits(const info& inf, const void* oRESTRICT src_dds_buffer, size_t src_dds_size, mapped_subresource* oRESTRICT subresources, size_t num_subresources)
{
	map_bits(inf, src_dds_buffer, src_dds_size, (const_mapped_subresource*)subresources, num_subresources);
}

scoped_allocation encode_dds(const texel_buffer& b, const alpha_option& option, const compression& compression)
{
	//if (1) oTHROW(operation_not_supported, "dds not yet implemented");

	const auto inf = b.get_info();
	oCHECK(inf.mip_layout == mip_layout::none || inf.mip_layout == mip_layout::tight, "right and below mip layouts not supported");

	const bool is3d = false; // how to specify? probably .z != 0...
	const bool isbc = dds_is_block_compressed(to_dds_format(inf.format));
	const bool mips = inf.mip_layout != mip_layout::none;
	const bool iscube = false; // how to specify?

	const dds_pixel_format ddspf = GetPixelFormat(inf.format);
	const bool kHasDX10 = iscube || inf.array_size > 1 || ddspf.dwFourCC == MAKEFOURCC('D','X','1','0');

	const size_t bits_size = b.size();
	const size_t size = bits_size + sizeof(dds_signature) + sizeof(dds_header)
		+ (kHasDX10 ? sizeof(dds_header_dx10) : 0);

	scoped_allocation a(default_allocate(size, 0), size, default_deallocate);

	*(uint*)a = dds_signature;
	auto h = (dds_header*)byte_add((void*)a, sizeof(dds_signature));
	auto d3d10ext = (dds_header_dx10*)&h[1];
	void* bits = kHasDX10 ? (void*)&d3d10ext[1] : (void*)&h[1];

	uint pitch = dds_calc_pitch(to_dds_format(inf.format), inf.dimensions.x);
	h->dwSize = sizeof(dds_header);
	h->dwFlags = DDS_HEADER_FLAGS_TEXTURE | (mips ? DDS_HEADER_FLAGS_MIPMAP : 0) | (isbc ? DDS_HEADER_FLAGS_LINEARSIZE : 0) | (is3d ? DDS_HEADER_FLAGS_VOLUME : 0);
	h->dwHeight = inf.dimensions.y;
	h->dwWidth = inf.dimensions.x;
	h->dwPitchOrLinearSize = isbc ? pitch : 0;
	h->dwDepth = is3d ? inf.dimensions.z : 0;
	h->dwMipMapCount = mips ? num_mips(inf) : 0;
	memset(h->dwReserved1, 0, sizeof(h->dwReserved1));
	h->ddspf = ddspf;
	h->dwCaps = DDS_SURFACE_FLAGS_TEXTURE | ((is3d || mips || inf.array_size) ? DDS_SURFACE_FLAGS_COMPLEX : 0) | (mips ? DDS_SURFACE_FLAGS_MIPMAP : 0);
	h->dwCaps2 = (iscube ? DDS_CUBEMAP_ALLFACES : 0) | (is3d ? DDS_FLAGS_VOLUME : 0);
	h->dwCaps3 = 0;
	h->dwCaps4 = 0;
	h->dwReserved2 = 0;
	if (kHasDX10)
	{
		d3d10ext->dxgiFormat = to_dds_format(inf.format);
		d3d10ext->resourceDimension = is3d ? DDS_RESOURCE_DIMENSION_TEXTURE3D : DDS_RESOURCE_DIMENSION_TEXTURE2D; // how to tell 1D? y==0 && z==0?
		d3d10ext->miscFlag = iscube ? DDS_RESOURCE_MISC_TEXTURECUBE : 0;
		d3d10ext->arraySize = is3d ? 1u : max(1u, inf.array_size); // DDS requires this to be non-zero
		d3d10ext->miscFlags2 = 0;
	}

	const int nSubresources = num_subresources(inf);
	mapped_subresource* subresources = (mapped_subresource*)default_allocate(sizeof(mapped_subresource) * nSubresources, 0);
	finally DeleteInitData([&] { if (subresources) default_deallocate(subresources); });
	map_bits(inf, bits, bits_size, subresources, nSubresources);

	for (int i = 0; i < nSubresources; i++)
		b.copy_to(i, subresources[i]);

	return a;
}

texel_buffer decode_dds(const void* buffer, size_t size, const alpha_option& option, const mip_layout& layout)
{
	//oCHECK(option == alpha_option::preserve, "changing alpha option not supported for dds");
	info inf = get_info_dds(buffer, size);
	oCHECK(inf.format != format::unknown, "invalid dds");
	texel_buffer b(inf);

	const uint nSubresources = num_subresources(inf);
	const_mapped_subresource* subresources = (const_mapped_subresource*)default_allocate(sizeof(const_mapped_subresource) * nSubresources, 0);
	finally DeleteInitData([&] { if (subresources) default_deallocate(subresources); });

	auto h = (const dds_header*)byte_add(buffer, sizeof(dds_signature));
	const void* bits = byte_add(buffer, sizeof(dds_signature) + sizeof(dds_header) + (has_dx10_header(*h) ? sizeof(dds_header_dx10) : 0));
	map_bits(inf, bits, size - byte_diff(bits, buffer), subresources, nSubresources);

	for (uint i = 0; i < nSubresources; i++)
		b.update_subresource(i, subresources[i]);

	return b;
}

}}
<|endoftext|>


<commit_before>/**
* (C) 2022 Jack Lloyd
*
* Botan is released under the Simplified BSD License (see license.txt)
*/

#include <botan/internal/argon2_ssse3.h>
#include <botan/internal/rotate.h>
#include <tmmintrin.h>

namespace Botan {

namespace {

class SIMD_2x64 final
   {
   public:
      SIMD_2x64& operator=(const SIMD_2x64& other) = default;
      SIMD_2x64(const SIMD_2x64& other) = default;

      SIMD_2x64& operator=(SIMD_2x64&& other) = default;
      SIMD_2x64(SIMD_2x64&& other) = default;

      SIMD_2x64() // zero initialized
         {
         m_simd = _mm_setzero_si128();
         }

      static SIMD_2x64 load_le(const void* in)
         {
         return SIMD_2x64(_mm_loadu_si128(reinterpret_cast<const __m128i*>(in)));
         }

      void store_le(uint64_t out[2]) const
         {
         this->store_le(reinterpret_cast<uint8_t*>(out));
         }

      void store_le(uint8_t out[]) const
         {
         _mm_storeu_si128(reinterpret_cast<__m128i*>(out), m_simd);
         }

      SIMD_2x64 operator+(const SIMD_2x64& other) const
         {
         SIMD_2x64 retval(*this);
         retval += other;
         return retval;
         }

      SIMD_2x64 operator^(const SIMD_2x64& other) const
         {
         SIMD_2x64 retval(*this);
         retval ^= other;
         return retval;
         }

      void operator+=(const SIMD_2x64& other)
         {
         m_simd = _mm_add_epi64(m_simd, other.m_simd);
         }

      void operator^=(const SIMD_2x64& other)
         {
         m_simd = _mm_xor_si128(m_simd, other.m_simd);
         }

      template<size_t ROT>
      BOTAN_FUNC_ISA("ssse3")
      SIMD_2x64 rotr() const
         {
         static_assert(ROT > 0 && ROT < 64, "Invalid rotation constant");

         if constexpr(ROT == 16)
            {
            auto tab = _mm_setr_epi8(2, 3, 4, 5, 6, 7, 0, 1, 10, 11, 12, 13, 14, 15, 8, 9);
            return SIMD_2x64(_mm_shuffle_epi8(m_simd, tab));
            }
         else if constexpr(ROT == 24)
            {
            auto tab = _mm_setr_epi8(3, 4, 5, 6, 7, 0, 1, 2, 11, 12, 13, 14, 15, 8, 9, 10);
            return SIMD_2x64(_mm_shuffle_epi8(m_simd, tab));
         }
         if constexpr(ROT == 32)
            {
            auto tab = _mm_setr_epi8(4, 5, 6, 7, 0, 1, 2, 3, 12, 13, 14, 15, 8, 9, 10, 11);
            return SIMD_2x64(_mm_shuffle_epi8(m_simd, tab));
            }

         return SIMD_2x64(_mm_or_si128(_mm_srli_epi64(m_simd, static_cast<int>(ROT)),
                                       _mm_slli_epi64(m_simd, static_cast<int>(64-ROT))));
         }

      template<size_t ROT>
      SIMD_2x64 rotl() const
         {
         return this->rotr<64-ROT>();
         }

      // Argon2 specific operation
      static SIMD_2x64 mul2_32(SIMD_2x64 x, SIMD_2x64 y)
         {
         const __m128i m = _mm_mul_epu32(x.m_simd, y.m_simd);
         return SIMD_2x64(_mm_add_epi64(m, m));
         }

      template<size_t T>
      BOTAN_FUNC_ISA("ssse3")
      static SIMD_2x64 alignr(SIMD_2x64 a, SIMD_2x64 b)
         {
         static_assert(T > 0 && T < 16, "Invalid alignr constant");
         return SIMD_2x64(_mm_alignr_epi8(a.m_simd, b.m_simd, T));
         }

      // Argon2 specific
      static void twist(
         SIMD_2x64& B0,
         SIMD_2x64& B1,
         SIMD_2x64& C0,
         SIMD_2x64& C1,
         SIMD_2x64& D0,
         SIMD_2x64& D1)
         {
         SIMD_2x64 T0, T1;

         T0 = SIMD_2x64::alignr<8>(B1, B0);
         T1 = SIMD_2x64::alignr<8>(B0, B1);
         B0 = T0;
         B1 = T1;

         T0 = C0;
         C0 = C1;
         C1 = T0;

         T0 = SIMD_2x64::alignr<8>(D0, D1);
         T1 = SIMD_2x64::alignr<8>(D1, D0);
         D0 = T0;
         D1 = T1;
         }

      // Argon2 specific
      static void untwist(
         SIMD_2x64& B0,
         SIMD_2x64& B1,
         SIMD_2x64& C0,
         SIMD_2x64& C1,
         SIMD_2x64& D0,
         SIMD_2x64& D1)
         {
         SIMD_2x64 T0, T1;

         T0 = SIMD_2x64::alignr<8>(B0, B1);
         T1 = SIMD_2x64::alignr<8>(B1, B0);
         B0 = T0;
         B1 = T1;

         T0 = C0;
         C0 = C1;
         C1 = T0;

         T0 = SIMD_2x64::alignr<8>(D1, D0);
         T1 = SIMD_2x64::alignr<8>(D0, D1);
         D0 = T0;
         D1 = T1;
         }

      explicit SIMD_2x64(__m128i x) : m_simd(x) {}
   private:
      __m128i m_simd;
   };

BOTAN_FORCE_INLINE void blamka_G(
   SIMD_2x64& A0,
   SIMD_2x64& A1,
   SIMD_2x64& B0,
   SIMD_2x64& B1,
   SIMD_2x64& C0,
   SIMD_2x64& C1,
   SIMD_2x64& D0,
   SIMD_2x64& D1)
   {
   A0 += B0 + SIMD_2x64::mul2_32(A0, B0);
   A1 += B1 + SIMD_2x64::mul2_32(A1, B1);
   D0 ^= A0;
   D1 ^= A1;
   D0 = D0.rotr<32>();
   D1 = D1.rotr<32>();

   C0 += D0 + SIMD_2x64::mul2_32(C0, D0);
   C1 += D1 + SIMD_2x64::mul2_32(C1, D1);
   B0 ^= C0;
   B1 ^= C1;
   B0 = B0.rotr<24>();
   B1 = B1.rotr<24>();

   A0 += B0 + SIMD_2x64::mul2_32(A0, B0);
   A1 += B1 + SIMD_2x64::mul2_32(A1, B1);
   D0 ^= A0;
   D1 ^= A1;
   D0 = D0.rotr<16>();
   D1 = D1.rotr<16>();

   C0 += D0 + SIMD_2x64::mul2_32(C0, D0);
   C1 += D1 + SIMD_2x64::mul2_32(C1, D1);
   B0 ^= C0;
   B1 ^= C1;
   B0 = B0.rotr<63>();
   B1 = B1.rotr<63>();
   }

BOTAN_FORCE_INLINE void blamka_R(
   SIMD_2x64& A0,
   SIMD_2x64& A1,
   SIMD_2x64& B0,
   SIMD_2x64& B1,
   SIMD_2x64& C0,
   SIMD_2x64& C1,
   SIMD_2x64& D0,
   SIMD_2x64& D1)
   {
   blamka_G(A0, A1, B0, B1, C0, C1, D0, D1);

   SIMD_2x64::twist(B0, B1, C0, C1, D0, D1);
   blamka_G(A0, A1, B0, B1, C0, C1, D0, D1);
   SIMD_2x64::untwist(B0, B1, C0, C1, D0, D1);
   }

}

void blamka_ssse3(uint64_t T[128])
   {
   for(size_t i = 0; i != 8; ++i)
      {
      SIMD_2x64 Tv[8];
      for(size_t j = 0; j != 8; ++j)
         Tv[j] = SIMD_2x64::load_le(&T[2*(8*i+j)]);

      blamka_R(Tv[0], Tv[1], Tv[2], Tv[3],
               Tv[4], Tv[5], Tv[6], Tv[7]);

      for(size_t j = 0; j != 8; ++j)
         Tv[j].store_le(&T[2*(8*i+j)]);
      }

   for(size_t i = 0; i != 8; ++i)
      {
      SIMD_2x64 Tv[8];
      for(size_t j = 0; j != 8; ++j)
         Tv[j] = SIMD_2x64::load_le(&T[2*(i+8*j)]);

      blamka_R(Tv[0], Tv[1], Tv[2], Tv[3],
               Tv[4], Tv[5], Tv[6], Tv[7]);

      for(size_t j = 0; j != 8; ++j)
         Tv[j].store_le(&T[2*(i+8*j)]);
      }
   }

}
<commit_msg>if/else<commit_after>/**
* (C) 2022 Jack Lloyd
*
* Botan is released under the Simplified BSD License (see license.txt)
*/

#include <botan/internal/argon2_ssse3.h>
#include <botan/internal/rotate.h>
#include <tmmintrin.h>

namespace Botan {

namespace {

class SIMD_2x64 final
   {
   public:
      SIMD_2x64& operator=(const SIMD_2x64& other) = default;
      SIMD_2x64(const SIMD_2x64& other) = default;

      SIMD_2x64& operator=(SIMD_2x64&& other) = default;
      SIMD_2x64(SIMD_2x64&& other) = default;

      SIMD_2x64() // zero initialized
         {
         m_simd = _mm_setzero_si128();
         }

      static SIMD_2x64 load_le(const void* in)
         {
         return SIMD_2x64(_mm_loadu_si128(reinterpret_cast<const __m128i*>(in)));
         }

      void store_le(uint64_t out[2]) const
         {
         this->store_le(reinterpret_cast<uint8_t*>(out));
         }

      void store_le(uint8_t out[]) const
         {
         _mm_storeu_si128(reinterpret_cast<__m128i*>(out), m_simd);
         }

      SIMD_2x64 operator+(const SIMD_2x64& other) const
         {
         SIMD_2x64 retval(*this);
         retval += other;
         return retval;
         }

      SIMD_2x64 operator^(const SIMD_2x64& other) const
         {
         SIMD_2x64 retval(*this);
         retval ^= other;
         return retval;
         }

      void operator+=(const SIMD_2x64& other)
         {
         m_simd = _mm_add_epi64(m_simd, other.m_simd);
         }

      void operator^=(const SIMD_2x64& other)
         {
         m_simd = _mm_xor_si128(m_simd, other.m_simd);
         }

      template<size_t ROT>
      BOTAN_FUNC_ISA("ssse3")
      SIMD_2x64 rotr() const
         {
         static_assert(ROT > 0 && ROT < 64, "Invalid rotation constant");

         if constexpr(ROT == 16)
            {
            auto tab = _mm_setr_epi8(2, 3, 4, 5, 6, 7, 0, 1, 10, 11, 12, 13, 14, 15, 8, 9);
            return SIMD_2x64(_mm_shuffle_epi8(m_simd, tab));
            }
         else if constexpr(ROT == 24)
            {
            auto tab = _mm_setr_epi8(3, 4, 5, 6, 7, 0, 1, 2, 11, 12, 13, 14, 15, 8, 9, 10);
            return SIMD_2x64(_mm_shuffle_epi8(m_simd, tab));
         }
         else if constexpr(ROT == 32)
            {
            auto tab = _mm_setr_epi8(4, 5, 6, 7, 0, 1, 2, 3, 12, 13, 14, 15, 8, 9, 10, 11);
            return SIMD_2x64(_mm_shuffle_epi8(m_simd, tab));
            }
         else
            {
            return SIMD_2x64(_mm_or_si128(_mm_srli_epi64(m_simd, static_cast<int>(ROT)),
                                          _mm_slli_epi64(m_simd, static_cast<int>(64-ROT))));
            }
         }

      template<size_t ROT>
      SIMD_2x64 rotl() const
         {
         return this->rotr<64-ROT>();
         }

      // Argon2 specific operation
      static SIMD_2x64 mul2_32(SIMD_2x64 x, SIMD_2x64 y)
         {
         const __m128i m = _mm_mul_epu32(x.m_simd, y.m_simd);
         return SIMD_2x64(_mm_add_epi64(m, m));
         }

      template<size_t T>
      BOTAN_FUNC_ISA("ssse3")
      static SIMD_2x64 alignr(SIMD_2x64 a, SIMD_2x64 b)
         {
         static_assert(T > 0 && T < 16, "Invalid alignr constant");
         return SIMD_2x64(_mm_alignr_epi8(a.m_simd, b.m_simd, T));
         }

      // Argon2 specific
      static void twist(
         SIMD_2x64& B0,
         SIMD_2x64& B1,
         SIMD_2x64& C0,
         SIMD_2x64& C1,
         SIMD_2x64& D0,
         SIMD_2x64& D1)
         {
         SIMD_2x64 T0, T1;

         T0 = SIMD_2x64::alignr<8>(B1, B0);
         T1 = SIMD_2x64::alignr<8>(B0, B1);
         B0 = T0;
         B1 = T1;

         T0 = C0;
         C0 = C1;
         C1 = T0;

         T0 = SIMD_2x64::alignr<8>(D0, D1);
         T1 = SIMD_2x64::alignr<8>(D1, D0);
         D0 = T0;
         D1 = T1;
         }

      // Argon2 specific
      static void untwist(
         SIMD_2x64& B0,
         SIMD_2x64& B1,
         SIMD_2x64& C0,
         SIMD_2x64& C1,
         SIMD_2x64& D0,
         SIMD_2x64& D1)
         {
         SIMD_2x64 T0, T1;

         T0 = SIMD_2x64::alignr<8>(B0, B1);
         T1 = SIMD_2x64::alignr<8>(B1, B0);
         B0 = T0;
         B1 = T1;

         T0 = C0;
         C0 = C1;
         C1 = T0;

         T0 = SIMD_2x64::alignr<8>(D1, D0);
         T1 = SIMD_2x64::alignr<8>(D0, D1);
         D0 = T0;
         D1 = T1;
         }

      explicit SIMD_2x64(__m128i x) : m_simd(x) {}
   private:
      __m128i m_simd;
   };

BOTAN_FORCE_INLINE void blamka_G(
   SIMD_2x64& A0,
   SIMD_2x64& A1,
   SIMD_2x64& B0,
   SIMD_2x64& B1,
   SIMD_2x64& C0,
   SIMD_2x64& C1,
   SIMD_2x64& D0,
   SIMD_2x64& D1)
   {
   A0 += B0 + SIMD_2x64::mul2_32(A0, B0);
   A1 += B1 + SIMD_2x64::mul2_32(A1, B1);
   D0 ^= A0;
   D1 ^= A1;
   D0 = D0.rotr<32>();
   D1 = D1.rotr<32>();

   C0 += D0 + SIMD_2x64::mul2_32(C0, D0);
   C1 += D1 + SIMD_2x64::mul2_32(C1, D1);
   B0 ^= C0;
   B1 ^= C1;
   B0 = B0.rotr<24>();
   B1 = B1.rotr<24>();

   A0 += B0 + SIMD_2x64::mul2_32(A0, B0);
   A1 += B1 + SIMD_2x64::mul2_32(A1, B1);
   D0 ^= A0;
   D1 ^= A1;
   D0 = D0.rotr<16>();
   D1 = D1.rotr<16>();

   C0 += D0 + SIMD_2x64::mul2_32(C0, D0);
   C1 += D1 + SIMD_2x64::mul2_32(C1, D1);
   B0 ^= C0;
   B1 ^= C1;
   B0 = B0.rotr<63>();
   B1 = B1.rotr<63>();
   }

BOTAN_FORCE_INLINE void blamka_R(
   SIMD_2x64& A0,
   SIMD_2x64& A1,
   SIMD_2x64& B0,
   SIMD_2x64& B1,
   SIMD_2x64& C0,
   SIMD_2x64& C1,
   SIMD_2x64& D0,
   SIMD_2x64& D1)
   {
   blamka_G(A0, A1, B0, B1, C0, C1, D0, D1);

   SIMD_2x64::twist(B0, B1, C0, C1, D0, D1);
   blamka_G(A0, A1, B0, B1, C0, C1, D0, D1);
   SIMD_2x64::untwist(B0, B1, C0, C1, D0, D1);
   }

}

void blamka_ssse3(uint64_t T[128])
   {
   for(size_t i = 0; i != 8; ++i)
      {
      SIMD_2x64 Tv[8];
      for(size_t j = 0; j != 8; ++j)
         Tv[j] = SIMD_2x64::load_le(&T[2*(8*i+j)]);

      blamka_R(Tv[0], Tv[1], Tv[2], Tv[3],
               Tv[4], Tv[5], Tv[6], Tv[7]);

      for(size_t j = 0; j != 8; ++j)
         Tv[j].store_le(&T[2*(8*i+j)]);
      }

   for(size_t i = 0; i != 8; ++i)
      {
      SIMD_2x64 Tv[8];
      for(size_t j = 0; j != 8; ++j)
         Tv[j] = SIMD_2x64::load_le(&T[2*(i+8*j)]);

      blamka_R(Tv[0], Tv[1], Tv[2], Tv[3],
               Tv[4], Tv[5], Tv[6], Tv[7]);

      for(size_t j = 0; j != 8; ++j)
         Tv[j].store_le(&T[2*(i+8*j)]);
      }
   }

}
<|endoftext|>