<commit_before>
#include <OpenSim/OpenSim.h>

using SimTK::Inertia;
using SimTK::Pi;
using SimTK::Vec3;

using OpenSim::Body;
using OpenSim::CoordinateSet;
using OpenSim::DisplayGeometry;
using OpenSim::Model;
using OpenSim::PinJoint;

/**
 * Creates an OpenSim model of a three-legged animal. The animal has only one
 * hind leg, centered mediolaterally.
 *
 * Axis orientations, in rough terms:
 * +x: direction of forward travel.
 * +y: up.
 * +z: out of the screen.
 * */

/** Helper functions. **/
void addCylinderDisplayGeometry(Body * body,
        const double & diam, const double & length)
{
    DisplayGeometry * geom = new DisplayGeometry("cylinder.vtp");
    SimTK::Rotation rot;
    // Rotate the cylinder's symmetry (Y) axis to align with the body's X axis:
    rot.setRotationFromAngleAboutZ(-0.5 * Pi);
    geom->setTransform(
            SimTK::Transform(rot, Vec3(0.5 * length, 0, 0)));

    geom->setScaleFactors(Vec3(0.5 * diam, length, 0.5 * diam));
    body->updDisplayer()->setShowAxes(true);
    body->updDisplayer()->updGeometrySet().adoptAndAppend(geom);
};

void addThighCoordinateLimitForce(Model & model, const std::string & coord)
{
    OpenSim::CoordinateLimitForce * force = new OpenSim::CoordinateLimitForce(
                coord, 90, 1.0E2, -90, 1.0E2, 1.0E1, 2.0, false);
	model.addForce(force);
};

void addShankCoordinateLimitForce(Model & model, const std::string & coord)
{
    OpenSim::CoordinateLimitForce * force = new OpenSim::CoordinateLimitForce(
                coord, 10, 1.0E2, -160, 1.0E2, 1.0E1, 2.0, false);
	model.addForce(force);
};

void addCoordinateActuator(Model & model, const std::string & coord)
{
    OpenSim::CoordinateActuator * act = new OpenSim::CoordinateActuator(coord);
    act->setName(coord + "_actuator");
    act->setMinControl(-1000.0);
    act->setMaxControl(1000.0);
    model.addForce(act);
};

int main(int argc, char * argv[])
{
    // Preliminaries.
    // --------------
    Model tripod;
    tripod.setName("tripod");

    // Properties.
    // -----------
    // Lengths, in meters.
    double torsoX = 0.5;
    double torsoY = 0.1;
    double torsoZ = 0.3;
    double thighLength = 0.1;
    double thighDiameter = 0.07;
    double shankLength = 0.1;
    double shankDiameter = 0.05;

    // Masses, in kilograms.
    double torsoMass = 1.0;
    double thighMass = 1.0;
    double shankMass = 1.0;

    // Center of mass, in meters.
    // This really just chooses the origin of our bodies.
    Vec3 torsoCOM(0, 0, 0);
    // We choose the x direction to be the axis of the leg segments, with +x
    // pointing toward the ground.
    Vec3 thighCOM(0.5 * thighLength, 0, 0);
    Vec3 shankCOM(0.5 * shankLength, 0, 0);

    // Moments of inertia, in kilograms-meters^2.
    // These are about the center of mass of the body, expressed in the frame
    // of the body.
    Inertia torsoInertia = Inertia::brick(
            0.5 * torsoX, 0.5 * torsoY, 0.5 * torsoZ);
    Inertia thighInertia = Inertia::cylinderAlongX(0.5 * thighDiameter,
            thighLength);
    Inertia shankInertia = Inertia::cylinderAlongX(0.5 * shankDiameter,
            shankLength);

    // Bodies.
    // -------
    Body * torso = new Body("torso",
            torsoMass, torsoCOM, torsoInertia);
    Body * hindThigh = new Body("hind_thigh",
            thighMass, thighCOM, thighInertia);
    Body * hindShank = new Body("hind_shank",
            shankMass, shankCOM, shankInertia);
    Body * frontLeftThigh = new Body("front_left_thigh",
            thighMass, thighCOM, thighInertia);
    Body * frontLeftShank = new Body("front_left_shank",
            shankMass, shankCOM, shankInertia);
    Body * frontRightThigh = new Body("front_right_thigh",
            thighMass, thighCOM, thighInertia);
    Body * frontRightShank = new Body("front_right_shank",
            shankMass, shankCOM, shankInertia);

    // Joints.
    // -------
    Body & ground = tripod.getGroundBody();

    // Ground -> Torso.
    // ````````````````
    // The torso has no constraints with respect to the ground.
    // By default, the tripod's feet are on the ground.
    Vec3 locGTinG(0, thighLength + shankLength, 0);
    Vec3 orientGTinG(0, 0, 0);
    Vec3 locGTinT(0, 0, 0);
    Vec3 orientGTinT(0, 0, 0);
    OpenSim::FreeJoint * groundTorso = new OpenSim::FreeJoint("torso",
            ground, locGTinG, orientGTinG, *torso, locGTinT, orientGTinT);

    CoordinateSet & groundTorsoCS = groundTorso->upd_CoordinateSet();

    // Rotation about x.
    groundTorsoCS[0].setName("torso_rx");
    double groundTorsoCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    groundTorsoCS[0].setRange(groundTorsoCS0range);

    groundTorsoCS[1].setName("torso_ry");
    double groundTorsoCS1range[2] = {-0.5 * Pi, 0.5 * Pi};
    groundTorsoCS[1].setRange(groundTorsoCS1range);

    groundTorsoCS[2].setName("torso_rz");
    double groundTorsoCS2range[2] = {-0.5 * Pi, 0.5 * Pi};
    groundTorsoCS[2].setRange(groundTorsoCS2range);

    // Translation in x.
    groundTorsoCS[3].setName("torso_tx");
    double groundTorsoCS3range[2] = {-1.0, 10.0};
    groundTorsoCS[3].setRange(groundTorsoCS3range);

    groundTorsoCS[4].setName("torso_ty");
    double groundTorsoCS4range[2] = {0.0, 2.0};
    groundTorsoCS[4].setRange(groundTorsoCS4range);

    groundTorsoCS[5].setName("torso_tz");
    double groundTorsoCS5range[2] = {-1.0, 1.0};
    groundTorsoCS[5].setRange(groundTorsoCS5range);

    // Torso -> hind thigh.
    // ````````````````````
    Vec3 locTHinT(-0.5 * torsoX, 0, 0);
    // By default, the leg is extended.
    Vec3 orientTHinT(0, 0, -0.5 * Pi);
    Vec3 locTHinH(0, 0, 0);
    Vec3 orientTHinH(0, 0, 0);
    PinJoint * torsoHindThigh = new PinJoint("hind_thigh",
            *torso, locTHinT, orientTHinT, *hindThigh, locTHinH, orientTHinH);

    CoordinateSet & torsoHindThighCS = torsoHindThigh->upd_CoordinateSet();
    torsoHindThighCS[0].setName("hind_thigh_flexion");
    double torsoHindThighCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    torsoHindThighCS[0].setRange(torsoHindThighCS0range);

    // Hind thigh -> hind shank.
    // `````````````````````````
    Vec3 locHTSinT(thighLength, 0, 0);
    Vec3 orientHTSinT(0, 0, 0);
    Vec3 locHTSinS(0, 0, 0);
    Vec3 orientHTSinS(0, 0, 0);
    PinJoint * hindThighShank = new PinJoint("hind_shank",
            *hindThigh, locHTSinT, orientHTSinT,
            *hindShank, locHTSinS, orientHTSinS);

    CoordinateSet & hindThighShankCS = hindThighShank->upd_CoordinateSet();
    hindThighShankCS[0].setName("hind_knee_extension");
    double hindThighShankCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    hindThighShankCS[0].setRange(hindThighShankCS0range);

    // Torso -> front left thigh.
    // ``````````````````````````
    Vec3 locTFLinT(0.5 * torsoX, 0, -0.5 * torsoZ);
    // By default, the leg is extended.
    Vec3 orientTFLinT(0, 0, -0.5 * Pi);
    Vec3 locTFLinFL(0, 0, 0);
    Vec3 orientTFLinFL(0, 0, 0);
    PinJoint * torsoFrontLeftThigh = new PinJoint("front_left_thigh",
            *torso, locTFLinT, orientTFLinT,
            *frontLeftThigh, locTFLinFL, orientTFLinFL);

    CoordinateSet & torsoFrontLeftThighCS =
        torsoFrontLeftThigh->upd_CoordinateSet();
    torsoFrontLeftThighCS[0].setName("front_left_thigh_flexion");
    double torsoFrontLeftThighCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    torsoFrontLeftThighCS[0].setRange(torsoFrontLeftThighCS0range);

    // Front left thigh -> front left shank.
    // `````````````````````````````````````
    Vec3 locFLTSinT(thighLength, 0, 0);
    Vec3 orientFLTSinT(0, 0, 0);
    Vec3 locFLTSinS(0, 0, 0);
    Vec3 orientFLTSinS(0, 0, 0);
    PinJoint * frontLeftThighShank = new PinJoint("front_left_shank",
            *frontLeftThigh, locFLTSinT, orientFLTSinT,
            *frontLeftShank, locFLTSinS, orientFLTSinS);

    CoordinateSet & frontLeftThighShankCS =
        frontLeftThighShank->upd_CoordinateSet();
    frontLeftThighShankCS[0].setName("front_left_knee_extension");
    double frontLeftThighShankCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    frontLeftThighShankCS[0].setRange(frontLeftThighShankCS0range);

    // Torso -> front right thigh.
    // ```````````````````````````
    Vec3 locTFRinT(0.5 * torsoX, 0, 0.5 * torsoZ);
    // By default, the leg is extended.
    Vec3 orientTFRinT(0, 0, -0.5 * Pi);
    Vec3 locTFRinFR(0, 0, 0);
    Vec3 orientTFRinFR(0, 0, 0);
    PinJoint * torsoFrontRightThigh = new PinJoint("front_right_thigh",
            *torso, locTFRinT, orientTFRinT,
            *frontRightThigh, locTFRinFR, orientTFRinFR);

    CoordinateSet & torsoFrontRightThighCS =
        torsoFrontRightThigh->upd_CoordinateSet();
    torsoFrontRightThighCS[0].setName("front_right_thigh_flexion");
    double torsoFrontRightThighCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    torsoFrontRightThighCS[0].setRange(torsoFrontRightThighCS0range);

    // Front right thigh -> front right shank.
    // ```````````````````````````````````````
    Vec3 locFRTSinT(thighLength, 0, 0);
    Vec3 orientFRTSinT(0, 0, 0);
    Vec3 locFRTSinS(0, 0, 0);
    Vec3 orientFRTSinS(0, 0, 0);
    PinJoint * frontRightThighShank = new PinJoint("front_right_shank",
            *frontRightThigh, locFRTSinT, orientFRTSinT,
            *frontRightShank, locFRTSinS, orientFRTSinS);

    CoordinateSet & frontRightThighShankCS =
        frontRightThighShank->upd_CoordinateSet();
    frontRightThighShankCS[0].setName("front_right_knee_extension");
    double frontRightThighShankCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    frontRightThighShankCS[0].setRange(frontRightThighShankCS0range);

    // Add bodies to the model.
    // ------------------------
    // Now that we've define the joints.
    tripod.addBody(torso);
    tripod.addBody(hindThigh);
    tripod.addBody(hindShank);
    tripod.addBody(frontLeftThigh);
    tripod.addBody(frontLeftShank);
    tripod.addBody(frontRightThigh);
    tripod.addBody(frontRightShank);

    // Display geometry.
    // -----------------

    // Torso.
    // ``````
    DisplayGeometry * torsoDisplay = new DisplayGeometry("box.vtp");
    torsoDisplay->setScaleFactors(
            Vec3(torsoX, torsoY,  torsoZ));
    torso->updDisplayer()->updGeometrySet().adoptAndAppend(torsoDisplay);
    torso->updDisplayer()->setShowAxes(true);

    // Limbs.
    // ``````
    addCylinderDisplayGeometry(hindThigh, thighDiameter, thighLength);
    addCylinderDisplayGeometry(hindShank, shankDiameter, shankLength);
    addCylinderDisplayGeometry(frontLeftThigh, thighDiameter, thighLength);
    addCylinderDisplayGeometry(frontLeftShank, shankDiameter, shankLength);
    addCylinderDisplayGeometry(frontRightThigh, thighDiameter, thighLength);
    addCylinderDisplayGeometry(frontRightShank, shankDiameter, shankLength);

    // Enforce joint limits on the legs.
    // ---------------------------------
    addThighCoordinateLimitForce(tripod, "hind_thigh_flexion");
    addThighCoordinateLimitForce(tripod, "front_left_thigh_flexion");
    addThighCoordinateLimitForce(tripod, "front_right_thigh_flexion");
    addShankCoordinateLimitForce(tripod, "hind_knee_extension");
    addShankCoordinateLimitForce(tripod, "front_left_knee_extension");
    addShankCoordinateLimitForce(tripod, "front_right_knee_extension");

    // Actuators.
    // ----------
    addCoordinateActuator(tripod, "hind_thigh_flexion");
    addCoordinateActuator(tripod, "front_left_thigh_flexion");
    addCoordinateActuator(tripod, "front_right_thigh_flexion");
    addCoordinateActuator(tripod, "hind_knee_extension");
    addCoordinateActuator(tripod, "front_left_knee_extension");
    addCoordinateActuator(tripod, "front_right_knee_extension");

    // Contact.
    // --------
    // So the tripod does not go through the ground.

    // Print the model.
    tripod.print("tripod.osim");

    return EXIT_SUCCESS;
}

<commit_msg>Add foot contact geometry. Note, none of the contact geometry shows up in GUI.<commit_after>
#include <OpenSim/OpenSim.h>

using SimTK::Inertia;
using SimTK::Pi;
using SimTK::Vec3;

using OpenSim::Body;
using OpenSim::CoordinateSet;
using OpenSim::DisplayGeometry;
using OpenSim::Model;
using OpenSim::PinJoint;

/**
 * Creates an OpenSim model of a three-legged animal. The animal has only one
 * hind leg, centered mediolaterally.
 *
 * Axis orientations, in rough terms:
 * +x: direction of forward travel.
 * +y: up.
 * +z: out of the screen.
 * */

/** Helper functions. **/
void addCylinderDisplayGeometry(Body * body,
        const double & diam, const double & length)
{
    DisplayGeometry * geom = new DisplayGeometry("cylinder.vtp");
    SimTK::Rotation rot;
    // Rotate the cylinder's symmetry (Y) axis to align with the body's X axis:
    rot.setRotationFromAngleAboutZ(-0.5 * Pi);
    geom->setTransform(
            SimTK::Transform(rot, Vec3(0.5 * length, 0, 0)));

    geom->setScaleFactors(Vec3(0.5 * diam, length, 0.5 * diam));
    body->updDisplayer()->setShowAxes(true);
    body->updDisplayer()->updGeometrySet().adoptAndAppend(geom);
};

void addThighCoordinateLimitForce(Model & model, const std::string & coord)
{
    OpenSim::CoordinateLimitForce * force = new OpenSim::CoordinateLimitForce(
                coord, 90, 1.0E2, -90, 1.0E2, 1.0E1, 2.0, false);
	model.addForce(force);
};

void addShankCoordinateLimitForce(Model & model, const std::string & coord)
{
    OpenSim::CoordinateLimitForce * force = new OpenSim::CoordinateLimitForce(
                coord, 10, 1.0E2, -160, 1.0E2, 1.0E1, 2.0, false);
	model.addForce(force);
};

void addCoordinateActuator(Model & model, const std::string & coord)
{
    OpenSim::CoordinateActuator * act = new OpenSim::CoordinateActuator(coord);
    act->setName(coord + "_actuator");
    act->setMinControl(-1000.0);
    act->setMaxControl(1000.0);
    model.addForce(act);
};

void addContactGeometry(Model & model, double xmeas, Body * body,
        const std::string name)
{
    OpenSim::ContactSphere * contact = new OpenSim::ContactSphere(
            0.05, Vec3(xmeas, 0, 0), *body, name);
    contact->setName(name);
    model.addContactGeometry(contact);
}

int main(int argc, char * argv[])
{
    // Preliminaries.
    // --------------
    Model tripod;
    tripod.setName("tripod");

    // Properties.
    // -----------
    // Lengths, in meters.
    double torsoX = 0.5;
    double torsoY = 0.1;
    double torsoZ = 0.3;
    double thighLength = 0.1;
    double thighDiameter = 0.07;
    double shankLength = 0.1;
    double shankDiameter = 0.05;

    // Masses, in kilograms.
    double torsoMass = 1.0;
    double thighMass = 1.0;
    double shankMass = 1.0;

    // Center of mass, in meters.
    // This really just chooses the origin of our bodies.
    Vec3 torsoCOM(0, 0, 0);
    // We choose the x direction to be the axis of the leg segments, with +x
    // pointing toward the ground.
    Vec3 thighCOM(0.5 * thighLength, 0, 0);
    Vec3 shankCOM(0.5 * shankLength, 0, 0);

    // Moments of inertia, in kilograms-meters^2.
    // These are about the center of mass of the body, expressed in the frame
    // of the body.
    Inertia torsoInertia = Inertia::brick(
            0.5 * torsoX, 0.5 * torsoY, 0.5 * torsoZ);
    Inertia thighInertia = Inertia::cylinderAlongX(0.5 * thighDiameter,
            thighLength);
    Inertia shankInertia = Inertia::cylinderAlongX(0.5 * shankDiameter,
            shankLength);

    // Bodies.
    // -------
    Body * torso = new Body("torso",
            torsoMass, torsoCOM, torsoInertia);
    Body * hindThigh = new Body("hind_thigh",
            thighMass, thighCOM, thighInertia);
    Body * hindShank = new Body("hind_shank",
            shankMass, shankCOM, shankInertia);
    Body * frontLeftThigh = new Body("front_left_thigh",
            thighMass, thighCOM, thighInertia);
    Body * frontLeftShank = new Body("front_left_shank",
            shankMass, shankCOM, shankInertia);
    Body * frontRightThigh = new Body("front_right_thigh",
            thighMass, thighCOM, thighInertia);
    Body * frontRightShank = new Body("front_right_shank",
            shankMass, shankCOM, shankInertia);

    // Joints.
    // -------
    Body & ground = tripod.getGroundBody();

    // Ground -> Torso.
    // ````````````````
    // The torso has no constraints with respect to the ground.
    // By default, the tripod's feet are on the ground.
    Vec3 locGTinG(0, thighLength + shankLength, 0);
    Vec3 orientGTinG(0, 0, 0);
    Vec3 locGTinT(0, 0, 0);
    Vec3 orientGTinT(0, 0, 0);
    OpenSim::FreeJoint * groundTorso = new OpenSim::FreeJoint("torso",
            ground, locGTinG, orientGTinG, *torso, locGTinT, orientGTinT);

    CoordinateSet & groundTorsoCS = groundTorso->upd_CoordinateSet();

    // Rotation about x.
    groundTorsoCS[0].setName("torso_rx");
    double groundTorsoCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    groundTorsoCS[0].setRange(groundTorsoCS0range);

    groundTorsoCS[1].setName("torso_ry");
    double groundTorsoCS1range[2] = {-0.5 * Pi, 0.5 * Pi};
    groundTorsoCS[1].setRange(groundTorsoCS1range);

    groundTorsoCS[2].setName("torso_rz");
    double groundTorsoCS2range[2] = {-0.5 * Pi, 0.5 * Pi};
    groundTorsoCS[2].setRange(groundTorsoCS2range);

    // Translation in x.
    groundTorsoCS[3].setName("torso_tx");
    double groundTorsoCS3range[2] = {-1.0, 10.0};
    groundTorsoCS[3].setRange(groundTorsoCS3range);

    groundTorsoCS[4].setName("torso_ty");
    double groundTorsoCS4range[2] = {0.0, 2.0};
    groundTorsoCS[4].setRange(groundTorsoCS4range);

    groundTorsoCS[5].setName("torso_tz");
    double groundTorsoCS5range[2] = {-1.0, 1.0};
    groundTorsoCS[5].setRange(groundTorsoCS5range);

    // Torso -> hind thigh.
    // ````````````````````
    Vec3 locTHinT(-0.5 * torsoX, 0, 0);
    // By default, the leg is extended.
    Vec3 orientTHinT(0, 0, -0.5 * Pi);
    Vec3 locTHinH(0, 0, 0);
    Vec3 orientTHinH(0, 0, 0);
    PinJoint * torsoHindThigh = new PinJoint("hind_thigh",
            *torso, locTHinT, orientTHinT, *hindThigh, locTHinH, orientTHinH);

    CoordinateSet & torsoHindThighCS = torsoHindThigh->upd_CoordinateSet();
    torsoHindThighCS[0].setName("hind_thigh_flexion");
    double torsoHindThighCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    torsoHindThighCS[0].setRange(torsoHindThighCS0range);

    // Hind thigh -> hind shank.
    // `````````````````````````
    Vec3 locHTSinT(thighLength, 0, 0);
    Vec3 orientHTSinT(0, 0, 0);
    Vec3 locHTSinS(0, 0, 0);
    Vec3 orientHTSinS(0, 0, 0);
    PinJoint * hindThighShank = new PinJoint("hind_shank",
            *hindThigh, locHTSinT, orientHTSinT,
            *hindShank, locHTSinS, orientHTSinS);

    CoordinateSet & hindThighShankCS = hindThighShank->upd_CoordinateSet();
    hindThighShankCS[0].setName("hind_knee_extension");
    double hindThighShankCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    hindThighShankCS[0].setRange(hindThighShankCS0range);

    // Torso -> front left thigh.
    // ``````````````````````````
    Vec3 locTFLinT(0.5 * torsoX, 0, -0.5 * torsoZ);
    // By default, the leg is extended.
    Vec3 orientTFLinT(0, 0, -0.5 * Pi);
    Vec3 locTFLinFL(0, 0, 0);
    Vec3 orientTFLinFL(0, 0, 0);
    PinJoint * torsoFrontLeftThigh = new PinJoint("front_left_thigh",
            *torso, locTFLinT, orientTFLinT,
            *frontLeftThigh, locTFLinFL, orientTFLinFL);

    CoordinateSet & torsoFrontLeftThighCS =
        torsoFrontLeftThigh->upd_CoordinateSet();
    torsoFrontLeftThighCS[0].setName("front_left_thigh_flexion");
    double torsoFrontLeftThighCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    torsoFrontLeftThighCS[0].setRange(torsoFrontLeftThighCS0range);

    // Front left thigh -> front left shank.
    // `````````````````````````````````````
    Vec3 locFLTSinT(thighLength, 0, 0);
    Vec3 orientFLTSinT(0, 0, 0);
    Vec3 locFLTSinS(0, 0, 0);
    Vec3 orientFLTSinS(0, 0, 0);
    PinJoint * frontLeftThighShank = new PinJoint("front_left_shank",
            *frontLeftThigh, locFLTSinT, orientFLTSinT,
            *frontLeftShank, locFLTSinS, orientFLTSinS);

    CoordinateSet & frontLeftThighShankCS =
        frontLeftThighShank->upd_CoordinateSet();
    frontLeftThighShankCS[0].setName("front_left_knee_extension");
    double frontLeftThighShankCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    frontLeftThighShankCS[0].setRange(frontLeftThighShankCS0range);

    // Torso -> front right thigh.
    // ```````````````````````````
    Vec3 locTFRinT(0.5 * torsoX, 0, 0.5 * torsoZ);
    // By default, the leg is extended.
    Vec3 orientTFRinT(0, 0, -0.5 * Pi);
    Vec3 locTFRinFR(0, 0, 0);
    Vec3 orientTFRinFR(0, 0, 0);
    PinJoint * torsoFrontRightThigh = new PinJoint("front_right_thigh",
            *torso, locTFRinT, orientTFRinT,
            *frontRightThigh, locTFRinFR, orientTFRinFR);

    CoordinateSet & torsoFrontRightThighCS =
        torsoFrontRightThigh->upd_CoordinateSet();
    torsoFrontRightThighCS[0].setName("front_right_thigh_flexion");
    double torsoFrontRightThighCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    torsoFrontRightThighCS[0].setRange(torsoFrontRightThighCS0range);

    // Front right thigh -> front right shank.
    // ```````````````````````````````````````
    Vec3 locFRTSinT(thighLength, 0, 0);
    Vec3 orientFRTSinT(0, 0, 0);
    Vec3 locFRTSinS(0, 0, 0);
    Vec3 orientFRTSinS(0, 0, 0);
    PinJoint * frontRightThighShank = new PinJoint("front_right_shank",
            *frontRightThigh, locFRTSinT, orientFRTSinT,
            *frontRightShank, locFRTSinS, orientFRTSinS);

    CoordinateSet & frontRightThighShankCS =
        frontRightThighShank->upd_CoordinateSet();
    frontRightThighShankCS[0].setName("front_right_knee_extension");
    double frontRightThighShankCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    frontRightThighShankCS[0].setRange(frontRightThighShankCS0range);

    // Add bodies to the model.
    // ------------------------
    // Now that we've define the joints.
    tripod.addBody(torso);
    tripod.addBody(hindThigh);
    tripod.addBody(hindShank);
    tripod.addBody(frontLeftThigh);
    tripod.addBody(frontLeftShank);
    tripod.addBody(frontRightThigh);
    tripod.addBody(frontRightShank);

    // Display geometry.
    // -----------------

    // Torso.
    // ``````
    DisplayGeometry * torsoDisplay = new DisplayGeometry("box.vtp");
    torsoDisplay->setScaleFactors(
            Vec3(torsoX, torsoY,  torsoZ));
    torso->updDisplayer()->updGeometrySet().adoptAndAppend(torsoDisplay);
    torso->updDisplayer()->setShowAxes(true);

    // Limbs.
    // ``````
    addCylinderDisplayGeometry(hindThigh, thighDiameter, thighLength);
    addCylinderDisplayGeometry(hindShank, shankDiameter, shankLength);
    addCylinderDisplayGeometry(frontLeftThigh, thighDiameter, thighLength);
    addCylinderDisplayGeometry(frontLeftShank, shankDiameter, shankLength);
    addCylinderDisplayGeometry(frontRightThigh, thighDiameter, thighLength);
    addCylinderDisplayGeometry(frontRightShank, shankDiameter, shankLength);

    // Enforce joint limits on the legs.
    // ---------------------------------
    addThighCoordinateLimitForce(tripod, "hind_thigh_flexion");
    addThighCoordinateLimitForce(tripod, "front_left_thigh_flexion");
    addThighCoordinateLimitForce(tripod, "front_right_thigh_flexion");
    addShankCoordinateLimitForce(tripod, "hind_knee_extension");
    addShankCoordinateLimitForce(tripod, "front_left_knee_extension");
    addShankCoordinateLimitForce(tripod, "front_right_knee_extension");

    // Actuators.
    // ----------
    addCoordinateActuator(tripod, "hind_thigh_flexion");
    addCoordinateActuator(tripod, "front_left_thigh_flexion");
    addCoordinateActuator(tripod, "front_right_thigh_flexion");
    addCoordinateActuator(tripod, "hind_knee_extension");
    addCoordinateActuator(tripod, "front_left_knee_extension");
    addCoordinateActuator(tripod, "front_right_knee_extension");

    // Contact.
    // --------
    // So the tripod does not go through the ground.
    OpenSim::ContactHalfSpace * groundContact = new OpenSim::ContactHalfSpace(
            Vec3(0), Vec3(0.0, 0.0, -0.5 * Pi), ground);
    groundContact->setName("ground_contact");
    tripod.addContactGeometry(groundContact);

    // Limbs.
    // ``````
    addContactGeometry(tripod, shankLength, hindShank, "hind_foot_contact");
    addContactGeometry(tripod, shankLength, frontLeftShank,
            "front_left_foot_contact");
    addContactGeometry(tripod, shankLength, frontRightShank,
            "front_right_foot_contact");

    // Print the model.
    tripod.print("tripod.osim");

    return EXIT_SUCCESS;
}

<|endoftext|>


<commit_before>
#include <OpenSim/OpenSim.h>

using SimTK::Inertia;
using SimTK::Pi;
using SimTK::Vec3;

using OpenSim::Body;
using OpenSim::CoordinateSet;
using OpenSim::DisplayGeometry;
using OpenSim::Model;
using OpenSim::PinJoint;

/**
 * Creates an OpenSim model of a three-legged animal. The animal has only one
 * hind leg, centered mediolaterally.
 *
 * Axis orientations, in rough terms:
 * +x: direction of forward travel.
 * +y: up.
 * +z: out of the screen.
 * */

/** Helper functions. **/
void addCylinderDisplayGeometry(Body * body,
        const double & diam, const double & length)
{
    DisplayGeometry * geom = new DisplayGeometry("cylinder.vtp");
    SimTK::Rotation rot;
    // Rotate the cylinder's symmetry (Y) axis to align with the body's X axis:
    rot.setRotationFromAngleAboutZ(-0.5 * Pi);
    geom->setTransform(
            SimTK::Transform(rot, Vec3(0.5 * length, 0, 0)));

    geom->setScaleFactors(Vec3(0.5 * diam, length, 0.5 * diam));
    body->updDisplayer()->setShowAxes(true);
    body->updDisplayer()->updGeometrySet().adoptAndAppend(geom);
};

void addThighCoordinateLimitForce(Model & model, const std::string & coord)
{
    OpenSim::CoordinateLimitForce * force = new OpenSim::CoordinateLimitForce(
                coord, 90, 1.0E2, -90, 1.0E2, 1.0E1, 2.0, false);
	model.addForce(force);
};

void addShankCoordinateLimitForce(Model & model, const std::string & coord)
{
    OpenSim::CoordinateLimitForce * force = new OpenSim::CoordinateLimitForce(
                coord, 10, 1.0E2, -160, 1.0E2, 1.0E1, 2.0, false);
	model.addForce(force);
};

int main(int argc, char * argv[])
{
    // Preliminaries.
    // --------------
    Model tripod;
    tripod.setName("tripod");

    // Properties.
    // -----------
    // Lengths, in meters.
    double torsoX = 0.5;
    double torsoY = 0.1;
    double torsoZ = 0.3;
    double thighLength = 0.1;
    double thighDiameter = 0.07;
    double shankLength = 0.1;
    double shankDiameter = 0.05;

    // Masses, in kilograms.
    double torsoMass = 1.0;
    double thighMass = 1.0;
    double shankMass = 1.0;

    // Center of mass, in meters.
    // This really just chooses the origin of our bodies.
    Vec3 torsoCOM(0, 0, 0);
    // We choose the x direction to be the axis of the leg segments, with +x
    // pointing toward the ground.
    Vec3 thighCOM(0.5 * thighLength, 0, 0);
    Vec3 shankCOM(0.5 * shankLength, 0, 0);

    // Moments of inertia, in kilograms-meters^2.
    // These are about the center of mass of the body, expressed in the frame
    // of the body.
    Inertia torsoInertia = Inertia::brick(
            0.5 * torsoX, 0.5 * torsoY, 0.5 * torsoZ);
    Inertia thighInertia = Inertia::cylinderAlongX(0.5 * thighDiameter,
            thighLength);
    Inertia shankInertia = Inertia::cylinderAlongX(0.5 * shankDiameter,
            shankLength);

    // Bodies.
    // -------
    Body * torso = new Body("torso",
            torsoMass, torsoCOM, torsoInertia);
    Body * hindThigh = new Body("hind_thigh",
            thighMass, thighCOM, thighInertia);
    Body * hindShank = new Body("hind_shank",
            shankMass, shankCOM, shankInertia);
    Body * frontLeftThigh = new Body("front_left_thigh",
            thighMass, thighCOM, thighInertia);
    Body * frontLeftShank = new Body("front_left_shank",
            shankMass, shankCOM, shankInertia);
    Body * frontRightThigh = new Body("front_right_thigh",
            thighMass, thighCOM, thighInertia);
    Body * frontRightShank = new Body("front_right_shank",
            shankMass, shankCOM, shankInertia);

    // Joints.
    // -------
    Body & ground = tripod.getGroundBody();

    // Ground -> Torso.
    // ````````````````
    // The torso has no constraints with respect to the ground.
    // By default, the tripod's feet are on the ground.
    Vec3 locGTinG(0, thighLength + shankLength, 0);
    Vec3 orientGTinG(0, 0, 0);
    Vec3 locGTinT(0, 0, 0);
    Vec3 orientGTinT(0, 0, 0);
    OpenSim::FreeJoint * groundTorso = new OpenSim::FreeJoint("torso",
            ground, locGTinG, orientGTinG, *torso, locGTinT, orientGTinT);

    CoordinateSet & groundTorsoCS = groundTorso->upd_CoordinateSet();

    // Rotation about x.
    groundTorsoCS[0].setName("torso_rx");
    double groundTorsoCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    groundTorsoCS[0].setRange(groundTorsoCS0range);

    groundTorsoCS[1].setName("torso_ry");
    double groundTorsoCS1range[2] = {-0.5 * Pi, 0.5 * Pi};
    groundTorsoCS[1].setRange(groundTorsoCS1range);

    groundTorsoCS[2].setName("torso_rz");
    double groundTorsoCS2range[2] = {-0.5 * Pi, 0.5 * Pi};
    groundTorsoCS[2].setRange(groundTorsoCS2range);

    // Translation in x.
    groundTorsoCS[3].setName("torso_tx");
    double groundTorsoCS3range[2] = {-1.0, 10.0};
    groundTorsoCS[3].setRange(groundTorsoCS3range);

    groundTorsoCS[4].setName("torso_ty");
    double groundTorsoCS4range[2] = {0.0, 2.0};
    groundTorsoCS[4].setRange(groundTorsoCS4range);

    groundTorsoCS[5].setName("torso_tz");
    double groundTorsoCS5range[2] = {-1.0, 1.0};
    groundTorsoCS[5].setRange(groundTorsoCS5range);

    // Torso -> hind thigh.
    // ````````````````````
    Vec3 locTHinT(-0.5 * torsoX, 0, 0);
    // By default, the leg is extended.
    Vec3 orientTHinT(0, 0, -0.5 * Pi);
    Vec3 locTHinH(0, 0, 0);
    Vec3 orientTHinH(0, 0, 0);
    PinJoint * torsoHindThigh = new PinJoint("hind_thigh",
            *torso, locTHinT, orientTHinT, *hindThigh, locTHinH, orientTHinH);

    CoordinateSet & torsoHindThighCS = torsoHindThigh->upd_CoordinateSet();
    torsoHindThighCS[0].setName("hind_thigh_flexion");
    double torsoHindThighCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    torsoHindThighCS[0].setRange(torsoHindThighCS0range);

    // Hind thigh -> hind shank.
    // `````````````````````````
    Vec3 locHTSinT(thighLength, 0, 0);
    Vec3 orientHTSinT(0, 0, 0);
    Vec3 locHTSinS(0, 0, 0);
    Vec3 orientHTSinS(0, 0, 0);
    PinJoint * hindThighShank = new PinJoint("hind_shank",
            *hindThigh, locHTSinT, orientHTSinT,
            *hindShank, locHTSinS, orientHTSinS);

    CoordinateSet & hindThighShankCS = hindThighShank->upd_CoordinateSet();
    hindThighShankCS[0].setName("hind_knee_extension");
    double hindThighShankCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    hindThighShankCS[0].setRange(hindThighShankCS0range);

    // Torso -> front left thigh.
    // ``````````````````````````
    Vec3 locTFLinT(0.5 * torsoX, 0, -0.5 * torsoZ);
    // By default, the leg is extended.
    Vec3 orientTFLinT(0, 0, -0.5 * Pi);
    Vec3 locTFLinFL(0, 0, 0);
    Vec3 orientTFLinFL(0, 0, 0);
    PinJoint * torsoFrontLeftThigh = new PinJoint("front_left_thigh",
            *torso, locTFLinT, orientTFLinT,
            *frontLeftThigh, locTFLinFL, orientTFLinFL);

    CoordinateSet & torsoFrontLeftThighCS =
        torsoFrontLeftThigh->upd_CoordinateSet();
    torsoFrontLeftThighCS[0].setName("front_left_thigh_flexion");
    double torsoFrontLeftThighCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    torsoFrontLeftThighCS[0].setRange(torsoFrontLeftThighCS0range);

    // Front left thigh -> front left shank.
    // `````````````````````````````````````
    Vec3 locFLTSinT(thighLength, 0, 0);
    Vec3 orientFLTSinT(0, 0, 0);
    Vec3 locFLTSinS(0, 0, 0);
    Vec3 orientFLTSinS(0, 0, 0);
    PinJoint * frontLeftThighShank = new PinJoint("front_left_shank",
            *frontLeftThigh, locFLTSinT, orientFLTSinT,
            *frontLeftShank, locFLTSinS, orientFLTSinS);

    CoordinateSet & frontLeftThighShankCS =
        frontLeftThighShank->upd_CoordinateSet();
    frontLeftThighShankCS[0].setName("front_left_knee_extension");
    double frontLeftThighShankCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    frontLeftThighShankCS[0].setRange(frontLeftThighShankCS0range);

    // Torso -> front right thigh.
    // ```````````````````````````
    Vec3 locTFRinT(0.5 * torsoX, 0, 0.5 * torsoZ);
    // By default, the leg is extended.
    Vec3 orientTFRinT(0, 0, -0.5 * Pi);
    Vec3 locTFRinFR(0, 0, 0);
    Vec3 orientTFRinFR(0, 0, 0);
    PinJoint * torsoFrontRightThigh = new PinJoint("front_right_thigh",
            *torso, locTFRinT, orientTFRinT,
            *frontRightThigh, locTFRinFR, orientTFRinFR);

    CoordinateSet & torsoFrontRightThighCS =
        torsoFrontRightThigh->upd_CoordinateSet();
    torsoFrontRightThighCS[0].setName("front_right_thigh_flexion");
    double torsoFrontRightThighCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    torsoFrontRightThighCS[0].setRange(torsoFrontRightThighCS0range);

    // Front right thigh -> front right shank.
    // ```````````````````````````````````````
    Vec3 locFRTSinT(thighLength, 0, 0);
    Vec3 orientFRTSinT(0, 0, 0);
    Vec3 locFRTSinS(0, 0, 0);
    Vec3 orientFRTSinS(0, 0, 0);
    PinJoint * frontRightThighShank = new PinJoint("front_right_shank",
            *frontRightThigh, locFRTSinT, orientFRTSinT,
            *frontRightShank, locFRTSinS, orientFRTSinS);

    CoordinateSet & frontRightThighShankCS =
        frontRightThighShank->upd_CoordinateSet();
    frontRightThighShankCS[0].setName("front_right_knee_extension");
    double frontRightThighShankCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    frontRightThighShankCS[0].setRange(frontRightThighShankCS0range);

    // Add bodies to the model.
    // ------------------------
    // Now that we've define the joints.
    tripod.addBody(torso);
    tripod.addBody(hindThigh);
    tripod.addBody(hindShank);
    tripod.addBody(frontLeftThigh);
    tripod.addBody(frontLeftShank);
    tripod.addBody(frontRightThigh);
    tripod.addBody(frontRightShank);

    // Display geometry.
    // -----------------

    // Torso.
    // ``````
    DisplayGeometry * torsoDisplay = new DisplayGeometry("box.vtp");
    torsoDisplay->setScaleFactors(
            Vec3(torsoX, torsoY,  torsoZ));
    torso->updDisplayer()->updGeometrySet().adoptAndAppend(torsoDisplay);
    torso->updDisplayer()->setShowAxes(true);

    addCylinderDisplayGeometry(hindThigh, thighDiameter, thighLength);
    addCylinderDisplayGeometry(hindShank, shankDiameter, shankLength);
    addCylinderDisplayGeometry(frontLeftThigh, thighDiameter, thighLength);
    addCylinderDisplayGeometry(frontLeftShank, shankDiameter, shankLength);
    addCylinderDisplayGeometry(frontRightThigh, thighDiameter, thighLength);
    addCylinderDisplayGeometry(frontRightShank, shankDiameter, shankLength);

    // Enforce joint limits on the legs.
    // ---------------------------------
    addThighCoordinateLimitForce(tripod, "hind_thigh_flexion");
    addThighCoordinateLimitForce(tripod, "front_left_thigh_flexion");
    addThighCoordinateLimitForce(tripod, "front_right_thigh_flexion");
    addShankCoordinateLimitForce(tripod, "hind_knee_extension");
    addShankCoordinateLimitForce(tripod, "front_left_knee_extension");
    addShankCoordinateLimitForce(tripod, "front_right_knee_extension");

    // Actuators.
    // ----------

    // Contact.
    // --------
    // So the tripod does not go through the ground.

    // Add actuators.
    // --------------
    tripod.print("tripod.osim");

    return EXIT_SUCCESS;
}

<commit_msg>Add coordinate actuators.<commit_after>
#include <OpenSim/OpenSim.h>

using SimTK::Inertia;
using SimTK::Pi;
using SimTK::Vec3;

using OpenSim::Body;
using OpenSim::CoordinateSet;
using OpenSim::DisplayGeometry;
using OpenSim::Model;
using OpenSim::PinJoint;

/**
 * Creates an OpenSim model of a three-legged animal. The animal has only one
 * hind leg, centered mediolaterally.
 *
 * Axis orientations, in rough terms:
 * +x: direction of forward travel.
 * +y: up.
 * +z: out of the screen.
 * */

/** Helper functions. **/
void addCylinderDisplayGeometry(Body * body,
        const double & diam, const double & length)
{
    DisplayGeometry * geom = new DisplayGeometry("cylinder.vtp");
    SimTK::Rotation rot;
    // Rotate the cylinder's symmetry (Y) axis to align with the body's X axis:
    rot.setRotationFromAngleAboutZ(-0.5 * Pi);
    geom->setTransform(
            SimTK::Transform(rot, Vec3(0.5 * length, 0, 0)));

    geom->setScaleFactors(Vec3(0.5 * diam, length, 0.5 * diam));
    body->updDisplayer()->setShowAxes(true);
    body->updDisplayer()->updGeometrySet().adoptAndAppend(geom);
};

void addThighCoordinateLimitForce(Model & model, const std::string & coord)
{
    OpenSim::CoordinateLimitForce * force = new OpenSim::CoordinateLimitForce(
                coord, 90, 1.0E2, -90, 1.0E2, 1.0E1, 2.0, false);
	model.addForce(force);
};

void addShankCoordinateLimitForce(Model & model, const std::string & coord)
{
    OpenSim::CoordinateLimitForce * force = new OpenSim::CoordinateLimitForce(
                coord, 10, 1.0E2, -160, 1.0E2, 1.0E1, 2.0, false);
	model.addForce(force);
};

void addCoordinateActuator(Model & model, const std::string & coord)
{
    OpenSim::CoordinateActuator * act = new OpenSim::CoordinateActuator(coord);
    act->setName(coord + "_actuator");
    act->setMinControl(-1000.0);
    act->setMaxControl(1000.0);
    model.addForce(act);
};

int main(int argc, char * argv[])
{
    // Preliminaries.
    // --------------
    Model tripod;
    tripod.setName("tripod");

    // Properties.
    // -----------
    // Lengths, in meters.
    double torsoX = 0.5;
    double torsoY = 0.1;
    double torsoZ = 0.3;
    double thighLength = 0.1;
    double thighDiameter = 0.07;
    double shankLength = 0.1;
    double shankDiameter = 0.05;

    // Masses, in kilograms.
    double torsoMass = 1.0;
    double thighMass = 1.0;
    double shankMass = 1.0;

    // Center of mass, in meters.
    // This really just chooses the origin of our bodies.
    Vec3 torsoCOM(0, 0, 0);
    // We choose the x direction to be the axis of the leg segments, with +x
    // pointing toward the ground.
    Vec3 thighCOM(0.5 * thighLength, 0, 0);
    Vec3 shankCOM(0.5 * shankLength, 0, 0);

    // Moments of inertia, in kilograms-meters^2.
    // These are about the center of mass of the body, expressed in the frame
    // of the body.
    Inertia torsoInertia = Inertia::brick(
            0.5 * torsoX, 0.5 * torsoY, 0.5 * torsoZ);
    Inertia thighInertia = Inertia::cylinderAlongX(0.5 * thighDiameter,
            thighLength);
    Inertia shankInertia = Inertia::cylinderAlongX(0.5 * shankDiameter,
            shankLength);

    // Bodies.
    // -------
    Body * torso = new Body("torso",
            torsoMass, torsoCOM, torsoInertia);
    Body * hindThigh = new Body("hind_thigh",
            thighMass, thighCOM, thighInertia);
    Body * hindShank = new Body("hind_shank",
            shankMass, shankCOM, shankInertia);
    Body * frontLeftThigh = new Body("front_left_thigh",
            thighMass, thighCOM, thighInertia);
    Body * frontLeftShank = new Body("front_left_shank",
            shankMass, shankCOM, shankInertia);
    Body * frontRightThigh = new Body("front_right_thigh",
            thighMass, thighCOM, thighInertia);
    Body * frontRightShank = new Body("front_right_shank",
            shankMass, shankCOM, shankInertia);

    // Joints.
    // -------
    Body & ground = tripod.getGroundBody();

    // Ground -> Torso.
    // ````````````````
    // The torso has no constraints with respect to the ground.
    // By default, the tripod's feet are on the ground.
    Vec3 locGTinG(0, thighLength + shankLength, 0);
    Vec3 orientGTinG(0, 0, 0);
    Vec3 locGTinT(0, 0, 0);
    Vec3 orientGTinT(0, 0, 0);
    OpenSim::FreeJoint * groundTorso = new OpenSim::FreeJoint("torso",
            ground, locGTinG, orientGTinG, *torso, locGTinT, orientGTinT);

    CoordinateSet & groundTorsoCS = groundTorso->upd_CoordinateSet();

    // Rotation about x.
    groundTorsoCS[0].setName("torso_rx");
    double groundTorsoCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    groundTorsoCS[0].setRange(groundTorsoCS0range);

    groundTorsoCS[1].setName("torso_ry");
    double groundTorsoCS1range[2] = {-0.5 * Pi, 0.5 * Pi};
    groundTorsoCS[1].setRange(groundTorsoCS1range);

    groundTorsoCS[2].setName("torso_rz");
    double groundTorsoCS2range[2] = {-0.5 * Pi, 0.5 * Pi};
    groundTorsoCS[2].setRange(groundTorsoCS2range);

    // Translation in x.
    groundTorsoCS[3].setName("torso_tx");
    double groundTorsoCS3range[2] = {-1.0, 10.0};
    groundTorsoCS[3].setRange(groundTorsoCS3range);

    groundTorsoCS[4].setName("torso_ty");
    double groundTorsoCS4range[2] = {0.0, 2.0};
    groundTorsoCS[4].setRange(groundTorsoCS4range);

    groundTorsoCS[5].setName("torso_tz");
    double groundTorsoCS5range[2] = {-1.0, 1.0};
    groundTorsoCS[5].setRange(groundTorsoCS5range);

    // Torso -> hind thigh.
    // ````````````````````
    Vec3 locTHinT(-0.5 * torsoX, 0, 0);
    // By default, the leg is extended.
    Vec3 orientTHinT(0, 0, -0.5 * Pi);
    Vec3 locTHinH(0, 0, 0);
    Vec3 orientTHinH(0, 0, 0);
    PinJoint * torsoHindThigh = new PinJoint("hind_thigh",
            *torso, locTHinT, orientTHinT, *hindThigh, locTHinH, orientTHinH);

    CoordinateSet & torsoHindThighCS = torsoHindThigh->upd_CoordinateSet();
    torsoHindThighCS[0].setName("hind_thigh_flexion");
    double torsoHindThighCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    torsoHindThighCS[0].setRange(torsoHindThighCS0range);

    // Hind thigh -> hind shank.
    // `````````````````````````
    Vec3 locHTSinT(thighLength, 0, 0);
    Vec3 orientHTSinT(0, 0, 0);
    Vec3 locHTSinS(0, 0, 0);
    Vec3 orientHTSinS(0, 0, 0);
    PinJoint * hindThighShank = new PinJoint("hind_shank",
            *hindThigh, locHTSinT, orientHTSinT,
            *hindShank, locHTSinS, orientHTSinS);

    CoordinateSet & hindThighShankCS = hindThighShank->upd_CoordinateSet();
    hindThighShankCS[0].setName("hind_knee_extension");
    double hindThighShankCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    hindThighShankCS[0].setRange(hindThighShankCS0range);

    // Torso -> front left thigh.
    // ``````````````````````````
    Vec3 locTFLinT(0.5 * torsoX, 0, -0.5 * torsoZ);
    // By default, the leg is extended.
    Vec3 orientTFLinT(0, 0, -0.5 * Pi);
    Vec3 locTFLinFL(0, 0, 0);
    Vec3 orientTFLinFL(0, 0, 0);
    PinJoint * torsoFrontLeftThigh = new PinJoint("front_left_thigh",
            *torso, locTFLinT, orientTFLinT,
            *frontLeftThigh, locTFLinFL, orientTFLinFL);

    CoordinateSet & torsoFrontLeftThighCS =
        torsoFrontLeftThigh->upd_CoordinateSet();
    torsoFrontLeftThighCS[0].setName("front_left_thigh_flexion");
    double torsoFrontLeftThighCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    torsoFrontLeftThighCS[0].setRange(torsoFrontLeftThighCS0range);

    // Front left thigh -> front left shank.
    // `````````````````````````````````````
    Vec3 locFLTSinT(thighLength, 0, 0);
    Vec3 orientFLTSinT(0, 0, 0);
    Vec3 locFLTSinS(0, 0, 0);
    Vec3 orientFLTSinS(0, 0, 0);
    PinJoint * frontLeftThighShank = new PinJoint("front_left_shank",
            *frontLeftThigh, locFLTSinT, orientFLTSinT,
            *frontLeftShank, locFLTSinS, orientFLTSinS);

    CoordinateSet & frontLeftThighShankCS =
        frontLeftThighShank->upd_CoordinateSet();
    frontLeftThighShankCS[0].setName("front_left_knee_extension");
    double frontLeftThighShankCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    frontLeftThighShankCS[0].setRange(frontLeftThighShankCS0range);

    // Torso -> front right thigh.
    // ```````````````````````````
    Vec3 locTFRinT(0.5 * torsoX, 0, 0.5 * torsoZ);
    // By default, the leg is extended.
    Vec3 orientTFRinT(0, 0, -0.5 * Pi);
    Vec3 locTFRinFR(0, 0, 0);
    Vec3 orientTFRinFR(0, 0, 0);
    PinJoint * torsoFrontRightThigh = new PinJoint("front_right_thigh",
            *torso, locTFRinT, orientTFRinT,
            *frontRightThigh, locTFRinFR, orientTFRinFR);

    CoordinateSet & torsoFrontRightThighCS =
        torsoFrontRightThigh->upd_CoordinateSet();
    torsoFrontRightThighCS[0].setName("front_right_thigh_flexion");
    double torsoFrontRightThighCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    torsoFrontRightThighCS[0].setRange(torsoFrontRightThighCS0range);

    // Front right thigh -> front right shank.
    // ```````````````````````````````````````
    Vec3 locFRTSinT(thighLength, 0, 0);
    Vec3 orientFRTSinT(0, 0, 0);
    Vec3 locFRTSinS(0, 0, 0);
    Vec3 orientFRTSinS(0, 0, 0);
    PinJoint * frontRightThighShank = new PinJoint("front_right_shank",
            *frontRightThigh, locFRTSinT, orientFRTSinT,
            *frontRightShank, locFRTSinS, orientFRTSinS);

    CoordinateSet & frontRightThighShankCS =
        frontRightThighShank->upd_CoordinateSet();
    frontRightThighShankCS[0].setName("front_right_knee_extension");
    double frontRightThighShankCS0range[2] = {-0.5 * Pi, 0.5 * Pi};
    frontRightThighShankCS[0].setRange(frontRightThighShankCS0range);

    // Add bodies to the model.
    // ------------------------
    // Now that we've define the joints.
    tripod.addBody(torso);
    tripod.addBody(hindThigh);
    tripod.addBody(hindShank);
    tripod.addBody(frontLeftThigh);
    tripod.addBody(frontLeftShank);
    tripod.addBody(frontRightThigh);
    tripod.addBody(frontRightShank);

    // Display geometry.
    // -----------------

    // Torso.
    // ``````
    DisplayGeometry * torsoDisplay = new DisplayGeometry("box.vtp");
    torsoDisplay->setScaleFactors(
            Vec3(torsoX, torsoY,  torsoZ));
    torso->updDisplayer()->updGeometrySet().adoptAndAppend(torsoDisplay);
    torso->updDisplayer()->setShowAxes(true);

    addCylinderDisplayGeometry(hindThigh, thighDiameter, thighLength);
    addCylinderDisplayGeometry(hindShank, shankDiameter, shankLength);
    addCylinderDisplayGeometry(frontLeftThigh, thighDiameter, thighLength);
    addCylinderDisplayGeometry(frontLeftShank, shankDiameter, shankLength);
    addCylinderDisplayGeometry(frontRightThigh, thighDiameter, thighLength);
    addCylinderDisplayGeometry(frontRightShank, shankDiameter, shankLength);

    // Enforce joint limits on the legs.
    // ---------------------------------
    addThighCoordinateLimitForce(tripod, "hind_thigh_flexion");
    addThighCoordinateLimitForce(tripod, "front_left_thigh_flexion");
    addThighCoordinateLimitForce(tripod, "front_right_thigh_flexion");
    addShankCoordinateLimitForce(tripod, "hind_knee_extension");
    addShankCoordinateLimitForce(tripod, "front_left_knee_extension");
    addShankCoordinateLimitForce(tripod, "front_right_knee_extension");

    // Actuators.
    // ----------

    // Contact.
    // --------
    // So the tripod does not go through the ground.

    addCoordinateActuator(tripod, "hind_thigh_flexion");
    addCoordinateActuator(tripod, "front_left_thigh_flexion");
    addCoordinateActuator(tripod, "front_right_thigh_flexion");
    addCoordinateActuator(tripod, "hind_knee_extension");
    addCoordinateActuator(tripod, "front_left_knee_extension");
    addCoordinateActuator(tripod, "front_right_knee_extension");

    // Print the model.
    tripod.print("tripod.osim");

    return EXIT_SUCCESS;
}

<|endoftext|>


<commit_before>#define CATCH_CONFIG_MAIN
#include <catch.hpp>

#include <vector>

#include <core/animations/map.hpp>

class CountAnimation: public Animation<int>
{
public:
    CountAnimation(int initialValue):
        currentValue(initialValue)
    {}

    virtual int nextValue(const int32_t) override
    {
        return currentValue++;
    }

    virtual int getCurrentValue() const override
    {
        return currentValue;
    }

    virtual bool isFinished() const override
    {
        return false;
    }

    virtual void reset(const int &initialValue) override
    {
        currentValue = initialValue;
    }

private:
    int currentValue;
};

AnimationPtr<int> makeCountMap(int initialValue)
{
    return AnimationPtr<int>(
        new Map<int, int>(AnimationPtr<int>(new CountAnimation(initialValue)),
                          [](const int& x) { return x * 2; },
                          [](const int& x) { return x / 2; }));
}

TEST_CASE("Map::nextValue should map the value the wrapped animation", "[map]") {
    const std::array<int, 5> expected = { 0, 2, 4, 6, 8 };
    auto map = makeCountMap(0);

    for (int x: expected) {
        REQUIRE ( x == map->nextValue(10) );
    }

    for (size_t i = 0; i < 5; ++i) {
        REQUIRE ( 10 == map->getCurrentValue() );
    }
}
<commit_msg>Add UT for Map::isFinished() (#111)<commit_after>#define CATCH_CONFIG_MAIN
#include <catch.hpp>

#include <vector>

#include <core/animations/map.hpp>

class CountAnimation: public Animation<int>
{
public:
    CountAnimation(int initialValue, int finalValue):
        m_finalValue(finalValue),
        m_currentValue(initialValue)
    {}

    virtual int nextValue(const int32_t) override
    {
        if (!isFinished()) {
            return m_currentValue++;
        } else {
            return getCurrentValue();
        }
    }

    virtual int getCurrentValue() const override
    {
        return m_currentValue;
    }

    virtual bool isFinished() const override
    {
        return m_currentValue >= m_finalValue;
    }

    virtual void reset(const int &initialValue) override
    {
        m_currentValue = initialValue;
    }

private:
    const int m_finalValue;
    int m_currentValue;
};

AnimationPtr<int> makeCountMap(int initialValue, int finalState)
{
    return AnimationPtr<int>(
        new Map<int, int>(AnimationPtr<int>(new CountAnimation(initialValue, finalState)),
                          [](const int& x) { return x * 2; },
                          [](const int& x) { return x / 2; }));
}

TEST_CASE("Map::nextValue() and Map::getCurrentValue() should map the value the wrapped animation", "[map]") {
    const std::array<int, 5> expected = { 0, 2, 4, 6, 8 };
    auto map = makeCountMap(0, 100);

    for (int x: expected) {
        REQUIRE ( x == map->nextValue(10) );
    }

    for (size_t i = 0; i < 5; ++i) {
        REQUIRE ( 10 == map->getCurrentValue() );
    }
}

TEST_CASE("Map::isFinished() should be equal to isFinished() of the wrapped animation", "[map]") {
    auto map = makeCountMap(0, 1);
    REQUIRE ( !map->isFinished() );
    REQUIRE ( 0 == map->nextValue(10) );
    REQUIRE ( map->isFinished() );
}
<|endoftext|>


<commit_before>#include <cstdio>
#include <algorithm>
#include <limits>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/wait.h>
#include <netinet/in.h>
#include <unistd.h>
#include <signal.h>

#include "server.h"
#include "stinger_net/stinger_server_state.h"

extern "C" {
#include "stinger_core/stinger.h"
#include "stinger_core/stinger_shared.h"
#include "stinger_core/xmalloc.h"
#include "stinger_utils/stinger_utils.h"
#include "stinger_utils/timer.h"
#include "stinger_utils/dimacs_support.h"
#include "stinger_utils/metisish_support.h"
#include "stinger_utils/json_support.h"
#include "stinger_utils/csv.h"
}

using namespace gt::stinger;

static char * graph_name = NULL;
static size_t graph_sz = 0;
static struct stinger * S = NULL;

int main(int argc, char *argv[])
{
  StingerServerState & server_state = StingerServerState::get_server_state();

  /* default global options */
  int port_names = 10101;
  int port_streams = port_names + 1;
  int port_algs = port_names + 2;
  int unleash_daemon = 0;

  graph_name = (char *) xmalloc (128*sizeof(char));
  sprintf(graph_name, "/stinger-default");
  char * input_file = (char *) xmalloc (1024*sizeof(char));
  input_file[0] = '\0';
  char * file_type = (char *) xmalloc (128*sizeof(char));
  file_type[0] = '\0';
  int use_numerics = 0;

  /* parse command line configuration */
  int opt = 0;
  while(-1 != (opt = getopt(argc, argv, "a:s:p:b:n:i:t:1h?dkvc:f:"))) {
    switch(opt) {
      case 'p': {
		  port_names = atoi(optarg);
		} break;

      case 'a': {
		  port_algs = atoi(optarg);
		} break;
      case 's': {
		  port_streams = atoi(optarg);
		} break;

      case 'n': {
		  strcpy (graph_name, optarg);
		} break;

      case 'k': {
		  server_state.set_write_alg_data(true);
		} break;

      case 'v': {
		  server_state.set_write_names(true);
		} break;

      case 'c': {
		  server_state.set_history_cap(atol(optarg));
		} break;

      case 'f': {
		  server_state.set_out_dir(optarg);
		} break;

      case 'i': {
		  strcpy (input_file, optarg);
		} break;

      case 't': {
		  strcpy (file_type, optarg);
		} break;

      case '1': {
		  use_numerics = 1;
		} break;
      case 'd': {
		  unleash_daemon = 1;
		} break;

      case '?':
      case 'h': {
		  printf("Usage:    %s\n"
		         "   [-p port_names]\n"
			 "   [-a port_algs]\n"
			 "   [-s port_streams]\n"
			 "   [-n graph_name]\n"
			 "   [-i input_file_path]\n"
			 "   [-t file_type]\n"
			 "   [-1 (for numeric IDs)]\n"
			 "   [-d daemon mode]\n"
			 "   [-k write algorithm states to disk]\n"
			 "   [-v write vertex name mapping to disk]\n"
			 "   [-f output directory for vertex names, alg states]\n"
			 "   [-c cap number of history files to keep per alg]  \n", argv[0]);
		  printf("Defaults:\n\tport_names: %d\n\tport_algs: %d\n\tport_streams: %d\n\tgraph_name: %s\n", port_names, port_algs, port_streams, graph_name);
		  exit(0);
		} break;

    }
  }

  /* print configuration to the terminal */
  printf("\tName: %s\n", graph_name);

  /* allocate the graph */
  struct stinger * S = stinger_shared_new(&graph_name);
  size_t graph_sz = S->length + sizeof(struct stinger);

  /* load edges from disk (if applicable) */
  if (input_file[0] != '\0')
  {
    printf("\tReading...");
    tic ();
    switch (file_type[0])
    {
      case 'b': {
		  int64_t nv, ne;
		  int64_t *off, *ind, *weight, *graphmem;
		  snarf_graph (input_file, &nv, &ne, (int64_t**)&off,
		      (int64_t**)&ind, (int64_t**)&weight, (int64_t**)&graphmem);
		  stinger_set_initial_edges (S, nv, 0, off, ind, weight, NULL, NULL, 0);
		  free(graphmem);
		} break;  /* STINGER binary */

      case 'c': {
		  load_csv_graph (S, input_file, use_numerics);
		} break;  /* CSV */

      case 'd': {
		  load_dimacs_graph (S, input_file);
		} break;  /* DIMACS */

      case 'm': {
		  load_metisish_graph (S, input_file);
		} break;

      case 'g': {
		} break;  /* GML / GraphML / GEXF -- you pick */

      case 'j': {
		  load_json_graph (S, input_file);
		} break;  /* JSON */

      case 'x': {
		} break;  /* XML */

      default:	{
		  printf("Unsupported file type.\n");
		  exit(0);
		} break;
    }
    printf(" (done: %lf s)\n", toc ());
  }

  printf("Graph created. Running stats...");
  tic();
  printf("\n\tVertices: %ld\n\tEdges: %ld\n",
      stinger_num_active_vertices(S), stinger_total_edges(S));

  /* consistency check */
  printf("\tConsistency %ld\n", (long) stinger_consistency_check(S, S->max_nv));
  printf("\tDone. %lf seconds\n", toc());

  /* initialize the singleton members */
  server_state.set_stinger(S);
  server_state.set_stinger_loc(graph_name);
  server_state.set_stinger_sz(graph_sz);
  server_state.set_port(port_names, port_streams, port_algs);
  server_state.set_mon_stinger(graph_name, sizeof(stinger_t) + S->length);

  /* we need a socket that can reply with the shmem name & size of the graph */
  pthread_t name_pid, batch_pid;

  /* this thread will handle the shared memory name mapping */
  pthread_create (&name_pid, NULL, start_udp_graph_name_server, NULL);

  /* this thread will handle the batch & alg servers */
  /* TODO: bring the thread creation for the alg server to this level */
  pthread_create (&batch_pid, NULL, start_tcp_batch_server, NULL);

  if(unleash_daemon) {
    while(1) { sleep(10); }
  } else {
    printf("Press <q> to shut down the server...\n");
    while (getchar() != 'q');
  }

  printf("Shutting down the name server..."); fflush(stdout);
  int status;
  kill(name_pid, SIGTERM);
  waitpid(name_pid, &status, 0);
  printf(" done.\n"); fflush(stdout);

  printf("Shutting down the batch server..."); fflush(stdout);
  kill(batch_pid, SIGTERM);
  waitpid(batch_pid, &status, 0);
  printf(" done.\n"); fflush(stdout);

  /* clean up */
  stinger_shared_free(S, graph_name, graph_sz);
  shmunlink(graph_name);
  free(graph_name);

  /* clean up algorithm data stores */
  for (size_t i = 0; i < server_state.get_num_algs(); i++) {
    StingerAlgState * alg_state = server_state.get_alg(i);
    const char * alg_data_loc = alg_state->data_loc.c_str();
    shmunlink(alg_data_loc);
  }

  return 0;
}
<commit_msg>Clean up after signals in the server.<commit_after>#include <cstdio>
#include <algorithm>
#include <limits>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/wait.h>
#include <netinet/in.h>
#include <unistd.h>
#include <signal.h>

#include "server.h"
#include "stinger_net/stinger_server_state.h"

extern "C" {
#include "stinger_core/stinger.h"
#include "stinger_core/stinger_shared.h"
#include "stinger_core/xmalloc.h"
#include "stinger_utils/stinger_utils.h"
#include "stinger_utils/timer.h"
#include "stinger_utils/dimacs_support.h"
#include "stinger_utils/metisish_support.h"
#include "stinger_utils/json_support.h"
#include "stinger_utils/csv.h"
}

using namespace gt::stinger;

static char * graph_name = NULL;
static size_t graph_sz = 0;
static struct stinger * S = NULL;

/* we need a socket that can reply with the shmem name & size of the graph */
static pthread_t name_pid, batch_pid;

static StingerServerState & server_state = StingerServerState::get_server_state();

static void cleanup (void);
extern "C" {
  static void sigterm_cleanup (int);
}

int main(int argc, char *argv[])
{
  /* default global options */
  int port_names = 10101;
  int port_streams = port_names + 1;
  int port_algs = port_names + 2;
  int unleash_daemon = 0;

  graph_name = (char *) xmalloc (128*sizeof(char));
  sprintf(graph_name, "/stinger-default");
  char * input_file = (char *) xmalloc (1024*sizeof(char));
  input_file[0] = '\0';
  char * file_type = (char *) xmalloc (128*sizeof(char));
  file_type[0] = '\0';
  int use_numerics = 0;

  /* parse command line configuration */
  int opt = 0;
  while(-1 != (opt = getopt(argc, argv, "a:s:p:b:n:i:t:1h?dkvc:f:"))) {
    switch(opt) {
      case 'p': {
		  port_names = atoi(optarg);
		} break;

      case 'a': {
		  port_algs = atoi(optarg);
		} break;
      case 's': {
		  port_streams = atoi(optarg);
		} break;

      case 'n': {
		  strcpy (graph_name, optarg);
		} break;

      case 'k': {
		  server_state.set_write_alg_data(true);
		} break;

      case 'v': {
		  server_state.set_write_names(true);
		} break;

      case 'c': {
		  server_state.set_history_cap(atol(optarg));
		} break;

      case 'f': {
		  server_state.set_out_dir(optarg);
		} break;

      case 'i': {
		  strcpy (input_file, optarg);
		} break;

      case 't': {
		  strcpy (file_type, optarg);
		} break;

      case '1': {
		  use_numerics = 1;
		} break;
      case 'd': {
		  unleash_daemon = 1;
		} break;

      case '?':
      case 'h': {
		  printf("Usage:    %s\n"
		         "   [-p port_names]\n"
			 "   [-a port_algs]\n"
			 "   [-s port_streams]\n"
			 "   [-n graph_name]\n"
			 "   [-i input_file_path]\n"
			 "   [-t file_type]\n"
			 "   [-1 (for numeric IDs)]\n"
			 "   [-d daemon mode]\n"
			 "   [-k write algorithm states to disk]\n"
			 "   [-v write vertex name mapping to disk]\n"
			 "   [-f output directory for vertex names, alg states]\n"
			 "   [-c cap number of history files to keep per alg]  \n", argv[0]);
		  printf("Defaults:\n\tport_names: %d\n\tport_algs: %d\n\tport_streams: %d\n\tgraph_name: %s\n", port_names, port_algs, port_streams, graph_name);
		  exit(0);
		} break;

    }
  }

  /* print configuration to the terminal */
  printf("\tName: %s\n", graph_name);

  /* allocate the graph */
  struct stinger * S = stinger_shared_new(&graph_name);
  size_t graph_sz = S->length + sizeof(struct stinger);

  /* load edges from disk (if applicable) */
  if (input_file[0] != '\0')
  {
    printf("\tReading...");
    tic ();
    switch (file_type[0])
    {
      case 'b': {
		  int64_t nv, ne;
		  int64_t *off, *ind, *weight, *graphmem;
		  snarf_graph (input_file, &nv, &ne, (int64_t**)&off,
		      (int64_t**)&ind, (int64_t**)&weight, (int64_t**)&graphmem);
		  stinger_set_initial_edges (S, nv, 0, off, ind, weight, NULL, NULL, 0);
		  free(graphmem);
		} break;  /* STINGER binary */

      case 'c': {
		  load_csv_graph (S, input_file, use_numerics);
		} break;  /* CSV */

      case 'd': {
		  load_dimacs_graph (S, input_file);
		} break;  /* DIMACS */

      case 'm': {
		  load_metisish_graph (S, input_file);
		} break;

      case 'g': {
		} break;  /* GML / GraphML / GEXF -- you pick */

      case 'j': {
		  load_json_graph (S, input_file);
		} break;  /* JSON */

      case 'x': {
		} break;  /* XML */

      default:	{
		  printf("Unsupported file type.\n");
		  exit(0);
		} break;
    }
    printf(" (done: %lf s)\n", toc ());
  }

  printf("Graph created. Running stats...");
  tic();
  printf("\n\tVertices: %ld\n\tEdges: %ld\n",
      stinger_num_active_vertices(S), stinger_total_edges(S));

  /* consistency check */
  printf("\tConsistency %ld\n", (long) stinger_consistency_check(S, S->max_nv));
  printf("\tDone. %lf seconds\n", toc());

  /* initialize the singleton members */
  server_state.set_stinger(S);
  server_state.set_stinger_loc(graph_name);
  server_state.set_stinger_sz(graph_sz);
  server_state.set_port(port_names, port_streams, port_algs);
  server_state.set_mon_stinger(graph_name, sizeof(stinger_t) + S->length);

  /* this thread will handle the shared memory name mapping */
  pthread_create (&name_pid, NULL, start_udp_graph_name_server, NULL);

  /* this thread will handle the batch & alg servers */
  /* TODO: bring the thread creation for the alg server to this level */
  pthread_create (&batch_pid, NULL, start_tcp_batch_server, NULL);

  {
    struct sigaction sa;
    sa.sa_flags = 0;
    sigemptyset (&sa.sa_mask);
    sa.sa_handler = sigterm_cleanup;
    /* Ignore the old handlers. */
    sigaction (SIGINT, &sa, NULL);
    sigaction (SIGTERM, &sa, NULL);
    sigaction (SIGHUP, &sa, NULL);
  }

  if(unleash_daemon) {
    while(1) { sleep(10); }
  } else {
    printf("Press <q> to shut down the server...\n");
    while (getchar() != 'q');
  }

  cleanup ();

  return 0;
}

void
cleanup (void)
{
  printf("Shutting down the name server..."); fflush(stdout);
  int status;
  kill(name_pid, SIGTERM);
  waitpid(name_pid, &status, 0);
  printf(" done.\n"); fflush(stdout);

  printf("Shutting down the batch server..."); fflush(stdout);
  kill(batch_pid, SIGTERM);
  waitpid(batch_pid, &status, 0);
  printf(" done.\n"); fflush(stdout);

  /* clean up */
  stinger_shared_free(S, graph_name, graph_sz);
  shmunlink(graph_name);
  free(graph_name);

  /* clean up algorithm data stores */
  for (size_t i = 0; i < server_state.get_num_algs(); i++) {
    StingerAlgState * alg_state = server_state.get_alg(i);
    const char * alg_data_loc = alg_state->data_loc.c_str();
    shmunlink(alg_data_loc);
  }
}

void
sigterm_cleanup (int)
{
  cleanup ();
  exit (EXIT_SUCCESS);
}
<|endoftext|>


<commit_before>#include "boost_test.h"
#include "ComplexFixedPoint.h"
#include <complex>

using namespace std;

BOOST_AUTO_TEST_CASE( CFxpAccessors )
{
	CFxp a(1, 2, 8);

	BOOST_CHECK_EQUAL(a.real(), 1);
	BOOST_CHECK_EQUAL(a.imag(), 2);
	BOOST_CHECK_EQUAL(a.width(), 8);
	BOOST_CHECK_EQUAL(a.minVal(), -128);
	BOOST_CHECK_EQUAL(a.maxVal(), 127);
}

BOOST_AUTO_TEST_CASE( CFxpAssignment )
{
	CFxp a(1, 2, 8);
	CFxp b(1, 2, 10);
	CFxp c;
	CFxp d(1, 2, 8, true);
	CFxp e(1, 2, 8, false);

	BOOST_CHECK_THROW(a = b, SizeMismatchException);
	BOOST_CHECK_NO_THROW(c = a);
	BOOST_CHECK_EQUAL(c, a);
	BOOST_CHECK_NO_THROW(d = b);
	BOOST_CHECK_EQUAL(d, b);
	BOOST_CHECK_THROW(e = b, SizeMismatchException);
}

BOOST_AUTO_TEST_CASE( CFxpEquality )
{
	CFxp a(1, 2, 8);
	CFxp b(1, 2, 8);
	CFxp c(1, 2, 10);
	CFxp d(13, 2, 8);
	CFxp e(1, 13, 8);
	CFxp f(24, 38, 21);

	/* same object as lhs and rhs */
	BOOST_CHECK_EQUAL(a == a, true);
	BOOST_CHECK_EQUAL(a != a, false);

	/* different equal objects */
	BOOST_CHECK_EQUAL(a == b, true);
	BOOST_CHECK_EQUAL(a != b, false);

	/* same value, different widths */
	BOOST_CHECK_EQUAL(a == c, false);
	BOOST_CHECK_EQUAL(a != c, true);

	/* different values, same widths */
	BOOST_CHECK_EQUAL(a == d, false);
	BOOST_CHECK_EQUAL(a != d, true);
	BOOST_CHECK_EQUAL(a == e, false);
	BOOST_CHECK_EQUAL(a != e, true);

	/* everything different */
	BOOST_CHECK_EQUAL(a == f, false);
	BOOST_CHECK_EQUAL(a != f, true);
}

BOOST_AUTO_TEST_CASE( CFxpScalarMultiplication )
{
	CFxp a(1, 2, 8);
	Fxp b(2, 5);

	/* multiplication by scalar is commutative */
	BOOST_CHECK_EQUAL(a * b, b * a);

	/* width after multiplication by scalar */
	BOOST_CHECK_EQUAL((a * b).width(), 13);

	/* result check */
	BOOST_CHECK_EQUAL(a * b, CFxp(2, 4, 13));
}
<commit_msg>Added unit tests for CFxp addition operator<commit_after>#include "boost_test.h"
#include "ComplexFixedPoint.h"
#include <complex>

using namespace std;

BOOST_AUTO_TEST_CASE( CFxpAccessors )
{
	CFxp a(1, 2, 8);

	BOOST_CHECK_EQUAL(a.real(), 1);
	BOOST_CHECK_EQUAL(a.imag(), 2);
	BOOST_CHECK_EQUAL(a.width(), 8);
	BOOST_CHECK_EQUAL(a.minVal(), -128);
	BOOST_CHECK_EQUAL(a.maxVal(), 127);
}

BOOST_AUTO_TEST_CASE( CFxpAssignment )
{
	CFxp a(1, 2, 8);
	CFxp b(1, 2, 10);
	CFxp c;
	CFxp d(1, 2, 8, true);
	CFxp e(1, 2, 8, false);

	BOOST_CHECK_THROW(a = b, SizeMismatchException);
	BOOST_CHECK_NO_THROW(c = a);
	BOOST_CHECK_EQUAL(c, a);
	BOOST_CHECK_NO_THROW(d = b);
	BOOST_CHECK_EQUAL(d, b);
	BOOST_CHECK_THROW(e = b, SizeMismatchException);
}

BOOST_AUTO_TEST_CASE( CFxpEquality )
{
	CFxp a(1, 2, 8);
	CFxp b(1, 2, 8);
	CFxp c(1, 2, 10);
	CFxp d(13, 2, 8);
	CFxp e(1, 13, 8);
	CFxp f(24, 38, 21);

	/* same object as lhs and rhs */
	BOOST_CHECK_EQUAL(a == a, true);
	BOOST_CHECK_EQUAL(a != a, false);

	/* different equal objects */
	BOOST_CHECK_EQUAL(a == b, true);
	BOOST_CHECK_EQUAL(a != b, false);

	/* same value, different widths */
	BOOST_CHECK_EQUAL(a == c, false);
	BOOST_CHECK_EQUAL(a != c, true);

	/* different values, same widths */
	BOOST_CHECK_EQUAL(a == d, false);
	BOOST_CHECK_EQUAL(a != d, true);
	BOOST_CHECK_EQUAL(a == e, false);
	BOOST_CHECK_EQUAL(a != e, true);

	/* everything different */
	BOOST_CHECK_EQUAL(a == f, false);
	BOOST_CHECK_EQUAL(a != f, true);
}

BOOST_AUTO_TEST_CASE( CFxpScalarMultiplication )
{
	CFxp a(1, 2, 8);
	Fxp b(2, 5);

	/* multiplication by scalar is commutative */
	BOOST_CHECK_EQUAL(a * b, b * a);

	/* width after multiplication by scalar */
	BOOST_CHECK_EQUAL((a * b).width(), 13);

	/* result check */
	BOOST_CHECK_EQUAL(a * b, CFxp(2, 4, 13));
}

BOOST_AUTO_TEST_CASE( CFxpAddition )
{
	CFxp a(1, 2, 8);
	CFxp b(2, 5, 5);

	/* addition is commutative */
	BOOST_CHECK_EQUAL(a + b, b + a);

	/* width after addition */
	BOOST_CHECK_EQUAL((a + b).width(), 9);

	/* result check */
	BOOST_CHECK_EQUAL(a + b, CFxp(3, 7, 9));
}
<|endoftext|>


<commit_before>/*
 *  BasicButton.cpp
 *  openFrameworks
 *
 *  Created by Matthias Rohrbach on 26.10.09.
 *  Copyright 2009 __MyCompanyName__. All rights reserved.
 *
 */

#include "BasicButton.h"

BasicButton::BasicButton() {
	isEnabled		= true;
	_isSelected		= false;
	hasActiveimage	= false;
	_isScalingImage = false;

	temp			= NULL;
	normal			= NULL;
	selected		= NULL;
	disabled		= NULL;
	active			= NULL;
	currentImage	= NULL;
}

void BasicButton::onFirstTouchDown(MultiTouchEvent& _event){
	if (isEnabled) {
		if(hasActiveimage){
			currentImage = active;
		}
		ofNotifyEvent(pressEvent, myEventArgs, this);
	}
}

void BasicButton::onLastTouchUp(MultiTouchEvent& _event){
	if (isEnabled) {
		currentImage = temp;
		ofNotifyEvent(releaseEvent, myEventArgs, this);
	}
}

void BasicButton::_draw(){
	if(normal == NULL){
		ofRect(0,0,width,height);
	}
	if (currentImage != NULL) {
		if (_isScalingImage) {
			currentImage->draw(0,0,width, height);
		} else {
			currentImage->draw((width  - currentImage->getWidth()) *.5,
							   (height - currentImage->getHeight())*.5);
		}

	}
}

void BasicButton::setImage(ofImage* _normal, ofImage* _selected, ofImage* _active, ofImage* _disabled){

	normal		= _normal;
	selected	= _selected;

	if(selected == NULL){
		selected	= normal;
	}

	active	= _active;

	hasActiveimage	= true;
	if(active == NULL){
		active			= normal;
		hasActiveimage	= false;
	}

	disabled=_disabled;
	if(disabled==NULL){
		disabled	= normal;
	}
	temp			= normal;
    currentImage	= normal;
}

void BasicButton::toggle(){
	if(_isSelected){
		deselect();
	}else{
		select();
	}
}

bool BasicButton::isSelected(){
	return _isSelected;
}

void BasicButton::select(){
	if(_isSelected || !isEnabled)return;
	_isSelected		= true;
	temp			= selected;
	currentImage	= selected;
}

void BasicButton::deselect(){
	if(!_isSelected || !isEnabled)return;
	_isSelected		= false;
	temp			= normal;
	currentImage	= normal;
}

void BasicButton::disable(){
	if (!isEnabled) return;
	isEnabled		= false;
	_isSelected		= false;
	temp			= disabled;
	currentImage	= disabled;
}

void BasicButton::enable(){
	if (isEnabled) return;
	isEnabled		= true;
	temp			= normal;
	currentImage	= normal;
}<commit_msg>Todo-comments. problems with alpha-blending...<commit_after>/*
 *  BasicButton.cpp
 *  openFrameworks
 *
 *  Created by Matthias Rohrbach on 26.10.09.
 *  Copyright 2009 __MyCompanyName__. All rights reserved.
 *
 */

#include "BasicButton.h"

BasicButton::BasicButton() {
	isEnabled		= true;
	_isSelected		= false;
	hasActiveimage	= false;
	_isScalingImage = false;

	temp			= NULL;
	normal			= NULL;
	selected		= NULL;
	disabled		= NULL;
	active			= NULL;
	currentImage	= NULL;
}

void BasicButton::onFirstTouchDown(MultiTouchEvent& _event){
	if (isEnabled) {
		if(hasActiveimage){
			currentImage = active;
		}
		ofNotifyEvent(pressEvent, myEventArgs, this);
	}
}

void BasicButton::onLastTouchUp(MultiTouchEvent& _event){
	if (isEnabled) {
		currentImage = temp;
		ofNotifyEvent(releaseEvent, myEventArgs, this);
	}
}

void BasicButton::_draw(){
	if(normal == NULL){
		ofRect(0,0,width,height);
	}

	if (currentImage != NULL) {
		//ofSetColor(color.r, color.g, color.b, 0);	// TODO: strange stuff with alpha
		if (_isScalingImage) {
			currentImage->draw(0,0,width, height);
		} else {
			currentImage->draw((width  - currentImage->getWidth()) *.5,
							   (height - currentImage->getHeight())*.5);
		}
	}
}

void BasicButton::setImage(ofImage* _normal, ofImage* _selected, ofImage* _active, ofImage* _disabled){

	normal		= _normal;
	selected	= _selected;

	if(selected == NULL){
		selected	= normal;
	}

	active	= _active;

	hasActiveimage	= true;
	if(active == NULL){
		active			= normal;
		hasActiveimage	= false;
	}

	disabled=_disabled;
	if(disabled==NULL){
		disabled	= normal;
	}
	temp			= normal;
    currentImage	= normal;
}

void BasicButton::toggle(){
	if(_isSelected){
		deselect();
	}else{
		select();
	}
}

bool BasicButton::isSelected(){
	return _isSelected;
}

void BasicButton::select(){
	if(_isSelected || !isEnabled)return;
	_isSelected		= true;
	temp			= selected;
	currentImage	= selected;
}

void BasicButton::deselect(){
	if(!_isSelected || !isEnabled)return;
	_isSelected		= false;
	temp			= normal;
	currentImage	= normal;
}

void BasicButton::disable(){
	if (!isEnabled) return;
	isEnabled		= false;
	_isSelected		= false;
	temp			= disabled;
	currentImage	= disabled;
}

void BasicButton::enable(){
	if (isEnabled) return;
	isEnabled		= true;
	temp			= normal;
	currentImage	= normal;
}<|endoftext|>


<commit_before>/*  This file is part of the KDE project.

Copyright (C) 2008 Nokia Corporation and/or its subsidiary(-ies).

This library is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 2.1 or 3 of the License.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with this library.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "backend.h"
#include "backendnode.h"

#include "audiooutput.h"
#include "effect.h"
#include "mediaobject.h"
#include "videowidget.h"
#include "volumeeffect.h"

//windows specific (DirectX Media Object)
#include <dmo.h>

#include <QtCore/QSettings>
#include <QtCore/QSet>
#include <QtCore/QVariant>

#include <QtCore/QtPlugin>

QT_BEGIN_NAMESPACE

Q_EXPORT_PLUGIN2(phonon_ds9, Phonon::DS9::Backend);

namespace Phonon
{
    namespace DS9
    {
        bool Backend::AudioMoniker::operator==(const AudioMoniker &other)
        {
            return other->IsEqual(*this) == S_OK;
        }

        Backend::Backend(QObject *parent, const QVariantList &)
            : QObject(parent)
        {
			::CoInitialize(0);

            //registering meta types
            qRegisterMetaType<HRESULT>("HRESULT");
            qRegisterMetaType<Graph>("Graph");
        }

        Backend::~Backend()
        {
            m_audioOutputs.clear();
            m_audioEffects.clear();
			::CoUninitialize();
        }

        QObject *Backend::createObject(BackendInterface::Class c, QObject *parent, const QList<QVariant> &args)
        {
            switch (c)
            {
            case MediaObjectClass:
                return new MediaObject(parent);
            case AudioOutputClass:
                return new AudioOutput(this, parent);
#ifndef QT_NO_PHONON_EFFECT
            case EffectClass:
                return new Effect(m_audioEffects[ args[0].toInt() ], parent);
#endif //QT_NO_PHONON_EFFECT
#ifndef QT_NO_PHONON_VIDEO
            case VideoWidgetClass:
                return new VideoWidget(qobject_cast<QWidget *>(parent));
#endif //QT_NO_PHONON_VIDEO
#ifndef QT_NO_PHONON_VOLUMEFADEREFFECT
            case VolumeFaderEffectClass:
                return new VolumeEffect(parent);
#endif //QT_NO_PHONON_VOLUMEFADEREFFECT
            default:
                return 0;
            }
        }

        bool Backend::supportsVideo() const
        {
#ifndef QT_NO_PHONON_VIDEO
            return true;
#else
            return false;
#endif //QT_NO_PHONON_VIDEO
        }

        QStringList Backend::availableMimeTypes() const
        {
            QStringList ret;
            {
                QSettings settings(QLatin1String("HKEY_LOCAL_MACHINE\\SOFTWARE\\Microsoft\\Multimedia\\mplayer2\\mime types"), QSettings::NativeFormat);
                ret += settings.childGroups();
            }
            {
                QSettings settings(QLatin1String("HKEY_LOCAL_MACHINE\\SOFTWARE\\Microsoft\\Multimedia\\wmplayer\\mime types"), QSettings::NativeFormat);
                ret += settings.childGroups();
            }

            ret.removeDuplicates();
            ret.replaceInStrings("\\", "/");
            qSort(ret);
            return ret;
        }

		Filter Backend::getAudioOutputFilter(int index) const
		{
			Filter ret;
			if (index >= 0 && index < m_audioOutputs.count()) {
				m_audioOutputs.at(index)->BindToObject(0, 0, IID_IBaseFilter, reinterpret_cast<void**>(&ret));
			} else {
				//just return the default audio renderer (not directsound)
				ret = Filter(CLSID_AudioRender, IID_IBaseFilter);
			}
			return ret;
		}

        QList<int> Backend::objectDescriptionIndexes(Phonon::ObjectDescriptionType type) const
        {
            QList<int> ret;

            switch(type)
            {
            case Phonon::AudioOutputDeviceType:
                {
#ifdef Q_OS_WINCE
					ret << 0; // only one audio device with index 0
#else
					ComPointer<ICreateDevEnum> devEnum(CLSID_SystemDeviceEnum, IID_ICreateDevEnum);
					if (!devEnum) {
						return ret; //it is impossible to enumerate the devices
					}
                    ComPointer<IEnumMoniker> enumMon;
                    HRESULT hr = devEnum->CreateClassEnumerator(CLSID_AudioRendererCategory, enumMon.pparam(), 0);
                    if (FAILED(hr)) {
                        break;
                    }
                    AudioMoniker mon;

                    //let's reorder the devices so that directshound appears first
                    int nbds = 0; //number of directsound devices

                    while (S_OK == enumMon->Next(1, mon.pparam(), 0)) {
                        LPOLESTR str = 0;
                        mon->GetDisplayName(0,0,&str);
                        const QString name = QString::fromUtf16((unsigned short*)str);
						ComPointer<IMalloc> alloc;
						::CoGetMalloc(1, alloc.pparam());
                        alloc->Free(str);

                        int insert_pos = 0;
                        if (!m_audioOutputs.contains(mon)) {
                            insert_pos = m_audioOutputs.count();
                            m_audioOutputs.append(mon);
                        } else {
                            insert_pos = m_audioOutputs.indexOf(mon);
                        }

                        if (name.contains(QLatin1String("DirectSound"))) {
                            ret.insert(nbds++, insert_pos);
                        } else {
                            ret.append(insert_pos);
                        }
                    }
#endif
					break;
                }
#ifndef QT_NO_PHONON_EFFECT
            case Phonon::EffectType:
                {
                    m_audioEffects.clear();
                    ComPointer<IEnumDMO> enumDMO;
                    HRESULT hr = ::DMOEnum(DMOCATEGORY_AUDIO_EFFECT, DMO_ENUMF_INCLUDE_KEYED, 0, 0, 0, 0, enumDMO.pparam());
                    if (SUCCEEDED(hr)) {
                        CLSID clsid;
                        while (S_OK == enumDMO->Next(1, &clsid, 0, 0)) {
                            ret += m_audioEffects.count();
                            m_audioEffects.append(clsid);
                        }
                    }
                    break;
                }
                break;
#endif //QT_NO_PHONON_EFFECT
            default:
                break;
            }
			return ret;
        }

        QHash<QByteArray, QVariant> Backend::objectDescriptionProperties(Phonon::ObjectDescriptionType type, int index) const
        {
            QHash<QByteArray, QVariant> ret;
            switch (type)
            {
            case Phonon::AudioOutputDeviceType:
                {
#ifdef Q_OS_WINCE
					ret["name"] = QLatin1String("default audio device");
#else
                    const AudioMoniker &mon = m_audioOutputs[index];
                    LPOLESTR str = 0;
                    HRESULT hr = mon->GetDisplayName(0,0, &str);
                    if (SUCCEEDED(hr)) {
                        QString name = QString::fromUtf16((unsigned short*)str);
						ComPointer<IMalloc> alloc;
						::CoGetMalloc(1, alloc.pparam());
                        alloc->Free(str);
                        ret["name"] = name.mid(name.indexOf('\\') + 1);
					}
#endif
                }
                break;
#ifndef QT_NO_PHONON_EFFECT
            case Phonon::EffectType:
                {
                    WCHAR name[80]; // 80 is clearly stated in the MSDN doc
                    HRESULT hr = ::DMOGetName(m_audioEffects[index], name);
                    if (SUCCEEDED(hr)) {
                        ret["name"] = QString::fromUtf16((unsigned short*)name);
                    }
                }
                break;
#endif //QT_NO_PHONON_EFFECT
            default:
                break;
            }
			return ret;
        }

        bool Backend::endConnectionChange(QSet<QObject *> objects)
        {
            if (objects.isEmpty())
                return true;
            //end of a transaction
            for(QSet<QObject *>::const_iterator it = objects.begin(); it != objects.end(); ++it) {
                if (BackendNode *node = qobject_cast<BackendNode*>(*it)) {
                    MediaObject *mo = node->mediaObject();
                    if (mo) {
                        switch(mo->transactionState)
                        {
                        case Phonon::ErrorState:
                        case Phonon::StoppedState:
                        case Phonon::LoadingState:
                            //nothing to do
                            break;
                        case Phonon::PausedState:
                            mo->pause();
                            break;
                        default:
                            mo->play();
                            break;
                        }

                        return mo->state() != Phonon::ErrorState;
                    }
                }
            }

            return false;
        }

        bool Backend::startConnectionChange(QSet<QObject *> objects)
        {
            if (objects.isEmpty())
                return true;

            //let's save the state of the graph (before we stop it)
            for(QSet<QObject *>::const_iterator it = objects.begin(); it != objects.end(); ++it) {
                if (BackendNode *node = qobject_cast<BackendNode*>(*it)) {
                    if (MediaObject *mo = node->mediaObject()) {
                        if (mo->state() != Phonon::StoppedState) {
                            mo->transactionState = mo->state();
                            mo->ensureStopped(); //we have to stop the graph..
                        }
                        return true;
                    }
                }
            }

            return false;
        }

        bool Backend::connectNodes(QObject *_source, QObject *_sink)
        {
            BackendNode *source = qobject_cast<BackendNode*>(_source);
            if (!source) {
                return false;
            }
            BackendNode *sink = qobject_cast<BackendNode*>(_sink);
            if (!sink) {
                return false;
            }

            //setting the graph if needed
            if (source->mediaObject() == 0 && sink->mediaObject() == 0) {
                    //error: no graph selected
                    return false;
            } else if (source->mediaObject() && source->mediaObject() != sink->mediaObject()) {
                //this' graph becomes the common one
                source->mediaObject()->grabNode(sink);
            } else if (source->mediaObject() == 0) {
                //sink's graph becomes the common one
                sink->mediaObject()->grabNode(source);
            }

            return source->mediaObject()->connectNodes(source, sink);
        }

        bool Backend::disconnectNodes(QObject *_source, QObject *_sink)
        {
            BackendNode *source = qobject_cast<BackendNode*>(_source);
            if (!source) {
                return false;
            }
            BackendNode *sink = qobject_cast<BackendNode*>(_sink);
            if (!sink) {
                return false;
            }

            return source->mediaObject() == 0 ||
                source->mediaObject()->disconnectNodes(source, sink);
        }
    }
}

QT_END_NAMESPACE

#include "moc_backend.cpp"
<commit_msg>qt4.4 compile++<commit_after>/*  This file is part of the KDE project.

Copyright (C) 2008 Nokia Corporation and/or its subsidiary(-ies).

This library is free software: you can redistribute it and/or modify
it under the terms of the GNU Lesser General Public License as published by
the Free Software Foundation, either version 2.1 or 3 of the License.

This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser General Public License for more details.

You should have received a copy of the GNU Lesser General Public License
along with this library.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "backend.h"
#include "backendnode.h"

#include "audiooutput.h"
#include "effect.h"
#include "mediaobject.h"
#include "videowidget.h"
#include "volumeeffect.h"

//windows specific (DirectX Media Object)
#include <dmo.h>

#include <QtCore/QSettings>
#include <QtCore/QSet>
#include <QtCore/QVariant>

#include <QtCore/QtPlugin>

QT_BEGIN_NAMESPACE

Q_EXPORT_PLUGIN2(phonon_ds9, Phonon::DS9::Backend);

namespace Phonon
{
    namespace DS9
    {
        bool Backend::AudioMoniker::operator==(const AudioMoniker &other)
        {
            return other->IsEqual(*this) == S_OK;
        }

        Backend::Backend(QObject *parent, const QVariantList &)
            : QObject(parent)
        {
			::CoInitialize(0);

            //registering meta types
            qRegisterMetaType<HRESULT>("HRESULT");
            qRegisterMetaType<Graph>("Graph");
        }

        Backend::~Backend()
        {
            m_audioOutputs.clear();
            m_audioEffects.clear();
			::CoUninitialize();
        }

        QObject *Backend::createObject(BackendInterface::Class c, QObject *parent, const QList<QVariant> &args)
        {
            switch (c)
            {
            case MediaObjectClass:
                return new MediaObject(parent);
            case AudioOutputClass:
                return new AudioOutput(this, parent);
#ifndef QT_NO_PHONON_EFFECT
            case EffectClass:
                return new Effect(m_audioEffects[ args[0].toInt() ], parent);
#endif //QT_NO_PHONON_EFFECT
#ifndef QT_NO_PHONON_VIDEO
            case VideoWidgetClass:
                return new VideoWidget(qobject_cast<QWidget *>(parent));
#endif //QT_NO_PHONON_VIDEO
#ifndef QT_NO_PHONON_VOLUMEFADEREFFECT
            case VolumeFaderEffectClass:
                return new VolumeEffect(parent);
#endif //QT_NO_PHONON_VOLUMEFADEREFFECT
            default:
                return 0;
            }
        }

        bool Backend::supportsVideo() const
        {
#ifndef QT_NO_PHONON_VIDEO
            return true;
#else
            return false;
#endif //QT_NO_PHONON_VIDEO
        }

        QStringList Backend::availableMimeTypes() const
        {
#if 0
// needs Qt4.5
            QStringList ret;
            {
                QSettings settings(QLatin1String("HKEY_LOCAL_MACHINE\\SOFTWARE\\Microsoft\\Multimedia\\mplayer2\\mime types"), QSettings::NativeFormat);
                ret += settings.childGroups();
            }
            {
                QSettings settings(QLatin1String("HKEY_LOCAL_MACHINE\\SOFTWARE\\Microsoft\\Multimedia\\wmplayer\\mime types"), QSettings::NativeFormat);
                ret += settings.childGroups();
            }

            ret.removeDuplicates();
#else
            QStringList ret;
            {
                QSettings settings(QLatin1String("HKEY_LOCAL_MACHINE\\SOFTWARE\\Microsoft\\Multimedia\\mplayer2\\mime types"), QSettings::NativeFormat);
                Q_FOREACH(const QString &s, settings.childGroups())
                  if(!ret.contains(s))
                    ret += s;
            }
            {
                QSettings settings(QLatin1String("HKEY_LOCAL_MACHINE\\SOFTWARE\\Microsoft\\Multimedia\\wmplayer\\mime types"), QSettings::NativeFormat);
                Q_FOREACH(const QString &s, settings.childGroups())
                  if(!ret.contains(s))
                    ret += s;
            }
#endif
            ret.replaceInStrings("\\", "/");
            qSort(ret);
            return ret;
        }

		Filter Backend::getAudioOutputFilter(int index) const
		{
			Filter ret;
			if (index >= 0 && index < m_audioOutputs.count()) {
				m_audioOutputs.at(index)->BindToObject(0, 0, IID_IBaseFilter, reinterpret_cast<void**>(&ret));
			} else {
				//just return the default audio renderer (not directsound)
				ret = Filter(CLSID_AudioRender, IID_IBaseFilter);
			}
			return ret;
		}

        QList<int> Backend::objectDescriptionIndexes(Phonon::ObjectDescriptionType type) const
        {
            QList<int> ret;

            switch(type)
            {
            case Phonon::AudioOutputDeviceType:
                {
#ifdef Q_OS_WINCE
					ret << 0; // only one audio device with index 0
#else
					ComPointer<ICreateDevEnum> devEnum(CLSID_SystemDeviceEnum, IID_ICreateDevEnum);
					if (!devEnum) {
						return ret; //it is impossible to enumerate the devices
					}
                    ComPointer<IEnumMoniker> enumMon;
                    HRESULT hr = devEnum->CreateClassEnumerator(CLSID_AudioRendererCategory, enumMon.pparam(), 0);
                    if (FAILED(hr)) {
                        break;
                    }
                    AudioMoniker mon;

                    //let's reorder the devices so that directshound appears first
                    int nbds = 0; //number of directsound devices

                    while (S_OK == enumMon->Next(1, mon.pparam(), 0)) {
                        LPOLESTR str = 0;
                        mon->GetDisplayName(0,0,&str);
                        const QString name = QString::fromUtf16((unsigned short*)str);
						ComPointer<IMalloc> alloc;
						::CoGetMalloc(1, alloc.pparam());
                        alloc->Free(str);

                        int insert_pos = 0;
                        if (!m_audioOutputs.contains(mon)) {
                            insert_pos = m_audioOutputs.count();
                            m_audioOutputs.append(mon);
                        } else {
                            insert_pos = m_audioOutputs.indexOf(mon);
                        }

                        if (name.contains(QLatin1String("DirectSound"))) {
                            ret.insert(nbds++, insert_pos);
                        } else {
                            ret.append(insert_pos);
                        }
                    }
#endif
					break;
                }
#ifndef QT_NO_PHONON_EFFECT
            case Phonon::EffectType:
                {
                    m_audioEffects.clear();
                    ComPointer<IEnumDMO> enumDMO;
                    HRESULT hr = ::DMOEnum(DMOCATEGORY_AUDIO_EFFECT, DMO_ENUMF_INCLUDE_KEYED, 0, 0, 0, 0, enumDMO.pparam());
                    if (SUCCEEDED(hr)) {
                        CLSID clsid;
                        while (S_OK == enumDMO->Next(1, &clsid, 0, 0)) {
                            ret += m_audioEffects.count();
                            m_audioEffects.append(clsid);
                        }
                    }
                    break;
                }
                break;
#endif //QT_NO_PHONON_EFFECT
            default:
                break;
            }
			return ret;
        }

        QHash<QByteArray, QVariant> Backend::objectDescriptionProperties(Phonon::ObjectDescriptionType type, int index) const
        {
            QHash<QByteArray, QVariant> ret;
            switch (type)
            {
            case Phonon::AudioOutputDeviceType:
                {
#ifdef Q_OS_WINCE
					ret["name"] = QLatin1String("default audio device");
#else
                    const AudioMoniker &mon = m_audioOutputs[index];
                    LPOLESTR str = 0;
                    HRESULT hr = mon->GetDisplayName(0,0, &str);
                    if (SUCCEEDED(hr)) {
                        QString name = QString::fromUtf16((unsigned short*)str);
						ComPointer<IMalloc> alloc;
						::CoGetMalloc(1, alloc.pparam());
                        alloc->Free(str);
                        ret["name"] = name.mid(name.indexOf('\\') + 1);
					}
#endif
                }
                break;
#ifndef QT_NO_PHONON_EFFECT
            case Phonon::EffectType:
                {
                    WCHAR name[80]; // 80 is clearly stated in the MSDN doc
                    HRESULT hr = ::DMOGetName(m_audioEffects[index], name);
                    if (SUCCEEDED(hr)) {
                        ret["name"] = QString::fromUtf16((unsigned short*)name);
                    }
                }
                break;
#endif //QT_NO_PHONON_EFFECT
            default:
                break;
            }
			return ret;
        }

        bool Backend::endConnectionChange(QSet<QObject *> objects)
        {
            if (objects.isEmpty())
                return true;
            //end of a transaction
            for(QSet<QObject *>::const_iterator it = objects.begin(); it != objects.end(); ++it) {
                if (BackendNode *node = qobject_cast<BackendNode*>(*it)) {
                    MediaObject *mo = node->mediaObject();
                    if (mo) {
                        switch(mo->transactionState)
                        {
                        case Phonon::ErrorState:
                        case Phonon::StoppedState:
                        case Phonon::LoadingState:
                            //nothing to do
                            break;
                        case Phonon::PausedState:
                            mo->pause();
                            break;
                        default:
                            mo->play();
                            break;
                        }

                        return mo->state() != Phonon::ErrorState;
                    }
                }
            }

            return false;
        }

        bool Backend::startConnectionChange(QSet<QObject *> objects)
        {
            if (objects.isEmpty())
                return true;

            //let's save the state of the graph (before we stop it)
            for(QSet<QObject *>::const_iterator it = objects.begin(); it != objects.end(); ++it) {
                if (BackendNode *node = qobject_cast<BackendNode*>(*it)) {
                    if (MediaObject *mo = node->mediaObject()) {
                        if (mo->state() != Phonon::StoppedState) {
                            mo->transactionState = mo->state();
                            mo->ensureStopped(); //we have to stop the graph..
                        }
                        return true;
                    }
                }
            }

            return false;
        }

        bool Backend::connectNodes(QObject *_source, QObject *_sink)
        {
            BackendNode *source = qobject_cast<BackendNode*>(_source);
            if (!source) {
                return false;
            }
            BackendNode *sink = qobject_cast<BackendNode*>(_sink);
            if (!sink) {
                return false;
            }

            //setting the graph if needed
            if (source->mediaObject() == 0 && sink->mediaObject() == 0) {
                    //error: no graph selected
                    return false;
            } else if (source->mediaObject() && source->mediaObject() != sink->mediaObject()) {
                //this' graph becomes the common one
                source->mediaObject()->grabNode(sink);
            } else if (source->mediaObject() == 0) {
                //sink's graph becomes the common one
                sink->mediaObject()->grabNode(source);
            }

            return source->mediaObject()->connectNodes(source, sink);
        }

        bool Backend::disconnectNodes(QObject *_source, QObject *_sink)
        {
            BackendNode *source = qobject_cast<BackendNode*>(_source);
            if (!source) {
                return false;
            }
            BackendNode *sink = qobject_cast<BackendNode*>(_sink);
            if (!sink) {
                return false;
            }

            return source->mediaObject() == 0 ||
                source->mediaObject()->disconnectNodes(source, sink);
        }
    }
}

QT_END_NAMESPACE

#include "moc_backend.cpp"
<|endoftext|>