SOP/SOP_IKSample.C

/*
 * Copyright (c) 2013
 *      Side Effects Software Inc.  All rights reserved.
 *
 * Redistribution and use of Houdini Development Kit samples in source and
 * binary forms, with or without modification, are permitted provided that the
 * following conditions are met:
 * 1. Redistributions of source code must retain the above copyright notice,
 *    this list of conditions and the following disclaimer.
 * 2. The name of Side Effects Software may not be used to endorse or
 *    promote products derived from this software without specific prior
 *    written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY SIDE EFFECTS SOFTWARE `AS IS' AND ANY EXPRESS
 * OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN
 * NO EVENT SHALL SIDE EFFECTS SOFTWARE BE LIABLE FOR ANY DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA,
 * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
 * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 *----------------------------------------------------------------------------
 *
 * The IKSample SOP
 *
 * Demonstrates example use of the Inverse Kinematics (IK) Solver found in the
 * KIN library.
 *
 */

#include "SOP_IKSample.h"
#include <GEO/GEO_Point.h>
#include <PRM/PRM_Include.h>
#include <OP/OP_Operator.h>
#include <OP/OP_OperatorTable.h>
#include <KIN/KIN_Bone.h>
#include <UT/UT_DSOVersion.h>
#include <UT/UT_Quaternion.h>
#include <UT/UT_Interrupt.h>
#include <UT/UT_Matrix4.h>
#include <UT/UT_RefArray.h>
#include <UT/UT_WorkBuffer.h>
#include <UT/UT_Vector3.h>
#include <stdlib.h>

using namespace HDK_Sample;

// Provide entry point for installing this SOP.
void
newSopOperator(OP_OperatorTable *table)
{
    OP_Operator *op = new OP_Operator(
                                "hdk_iksample",
                                "IKSample",
                                SOP_IKSample::myConstructor,
                                SOP_IKSample::myTemplateList,
                                1,      // min inputs
                                1       // max inputs
                                );
    table->addOperator(op);
}

OP_Node *
SOP_IKSample::myConstructor(OP_Network *net, const char *name, OP_Operator *op)
{
    return new SOP_IKSample(net, name, op);
}

// SOP Parameters.
static PRM_Name     names[] =
{
    PRM_Name("goal",        "Goal Position"),
    PRM_Name("twist",       "Twist"),
    PRM_Name("dampen",      "Dampening"),
    PRM_Name("straighten",  "Straighten Solution"),
    PRM_Name("threshold",   "Threshold"),
};
static PRM_Default  sopThresholdDefault(1e-03f);

PRM_Template        SOP_IKSample::myTemplateList[] =
{
    PRM_Template(PRM_FLT_J,     3, &names[0]),
    PRM_Template(PRM_ANGLE_J,   1, &names[1], 0, 0, &PRMangleRange),
    PRM_Template(PRM_FLT_J,     1, &names[2]),
    PRM_Template(PRM_TOGGLE_J,  1, &names[3]),
    PRM_Template(PRM_FLT_J,     1, &names[4], &sopThresholdDefault),
    PRM_Template() // sentinel
};

// Constructor
SOP_IKSample::SOP_IKSample(OP_Network *net, const char *name, OP_Operator *op)
        : SOP_Node(net, name, op)
{
}

// Destructor
SOP_IKSample::~SOP_IKSample()
{
}

// Evaluate parameters for the solver.
bool
SOP_IKSample::evaluateSolverParms(OP_Context &context, KIN_InverseParm &parms)
{
    fpreal  t = context.getTime();

    // the goal position is relative to chain root
    UT_Vector3R pos;
    GOAL(t, pos);
    parms.myEndAffectorPos = pos;
    UT_Vector3 origin = gdp->points()(0)->getPos();
    parms.myEndAffectorPos -= origin;

    parms.myTwist = TWIST(t);
    parms.myDampen = DAMPEN(t);
    parms.myResistStraight = STRAIGHTEN(t);
    parms.myTrackingThresholdFactor = THRESHOLD(t);
    parms.myTwistAffectorFlag = false;
    // if myTwistAffectorFlag is true, then also set the twist affector pos
    // parms.myInverseParms.myTwistAffectorPos = ...

    // Return true only if we didn't encounter any errors during parameter
    // evaluation.
    return (error() < UT_ERROR_ABORT);
}

// Setup myRestChain. The rest chain is used by the IK solver to determine
// the initial chain that is iteratively solved towards the goal position.
bool
SOP_IKSample::setupRestChain()
{
    int          num_points = gdp->points().entries();
    int          num_bones = num_points - 1;
    UT_Vector3R  prev_pos(0, 0,  0);
    UT_Vector3R  prev_dir(0, 0, -1);

    if (num_bones < 1)
    {
        UT_WorkBuffer str;
        str.sprintf("%d", 2 - num_points);
        addError(SOP_NEED_MORE_POINTS, str.buffer());
        return false;
    }

    myRestChain.setNbones(num_bones);
    prev_pos = gdp->points()[0]->getPos();
    for (int i = 0; i < num_bones; i++)
    {
        UT_Vector3R     pos = gdp->points()(i+1)->getPos();
        UT_Vector3R     dir = pos - prev_pos;
        fpreal          length = dir.length();
        fpreal          damp = 0;       // only used by "constraint" solver
        UT_Matrix4      pre_xform(1);   // assume identity
        void            *data = NULL;   // not used
        UT_Vector3R     rot;
        UT_Quaternion   quat;

        // Since we're dealing with only point positions, calculate a natural
        // twist rotation using quaternions from the previous bone.
        dir.normalize();
        quat.updateFromVectors(prev_dir, dir);
        rot = quat.computeRotations(KIN_Chain::getXformOrder());
        rot.radToDeg();

        // Update the bone in the chain.
        myRestChain.updateBone(i, length, rot.data(), damp, pre_xform, data);

        // If we're dealing with the "constraint" solver, then we need to
        // do more setup here.
        //myRestChain.setConstraint(i, ...);

        // Update position and direction for next iteration.
        prev_pos = pos;
        prev_dir = dir;
    }

    return true;
}

// Compute the output geometry for the SOP.
OP_ERROR
SOP_IKSample::cookMySop(OP_Context &context)
{
    GB_AttributeRef orient_attrib;

    // Before we do anything, we must lock our inputs. Before returning, we
    // have to make sure that the inputs get unlocked.
    if (lockInputs(context) >= UT_ERROR_ABORT)
        return error();

    // Setup the rest chain if needed.
    int input_changed;
    duplicateChangedSource(/*input*/0, context, &input_changed);
    if (input_changed)
    {
        if (!setupRestChain())
        {
            unlockInputs();
            return error();
        }

        // Create the "orient" attribute for storing our solved rotations.
        float def[4] = { 0, 0, 0, 1 };
        orient_attrib = gdp->addPointAttrib("orient", 4 * sizeof(float),
                                            GB_ATTRIB_FLOAT, &def[0]);

        // Compute the "pscale" attribute using the bone lengths for
        // completeness. This allows us to easily use the BoneLink SOP along
        // with the Copy SOP.
        GB_AttributeRef pscale_attrib;
        pscale_attrib = gdp->addPointAttrib("pscale", 1 * sizeof(float),
                                            GB_ATTRIB_FLOAT, &def[3]);
        GEO_Point *ppt;
        int i = 0;
        FOR_ALL_GPOINTS(gdp, ppt)
        {
            float length = 0;
            if (i < myRestChain.getNbones())
                length = myRestChain.getBone(i)->getLength();
            ppt->setValue<float>(pscale_attrib, length);
            i++;
        }
    }

    // Evaluate parameters for solver.
    const char *    solver_name = "inverse";
    KIN_InverseParm parms;
    if (!evaluateSolverParms(context, parms))
    {
        unlockInputs();
        return error();
    }

    // Perform solve.
    KIN_Chain solution;
    if (!myRestChain.solve(solver_name, &parms, solution))
    {
        addError(SOP_MESSAGE, "Failed to solve.");
        unlockInputs();
        return error();
    }

    // We store the solved orientations into the "orient" attribute that is
    // understood by the Copy SOP. We also compute "pscale" as well for
    // completeness. This allows us to easily use the BoneLink SOP along with
    // the Copy SOP.
    if (!orient_attrib.isValid())
    {
        orient_attrib = gdp->findPointAttrib("orient", 4 * sizeof(float),
                                      GB_ATTRIB_FLOAT);
        if (!orient_attrib.isValid())
        {
            addError(SOP_MESSAGE, "Failed to create orient attribute.");
            unlockInputs();
            return error();
        }
    }

    // Output geometry.
    UT_Matrix4R xform(1); // identity, this is the world transform
    int         num_bones = gdp->points().entries() - 1;
    fpreal      prev_length = 0;
    UT_Vector3  pos;

    pos = gdp->points()(0)->getPos();
    xform.setTranslates(pos);           // set chain origin

    for (int i = 0; i < num_bones; i++)
    {
        const KIN_Bone  *bone = solution.getBone(i);
        GEO_Point       *ppt = gdp->points()(i);

        // Since we never actually set any pre-transforms, this leftMult()
        // ends up doing nothing.
        xform.leftMult(bone->getExtraXform());

        // Take the bone length into account for the point position.
        xform.pretranslate(0, 0, -1 * prev_length);

        xform.getTranslates(pos);
        ppt->setPos(pos);

        // Update our world transform with the bone rotations. Note that
        // bone->getRotates() returns them in degrees.
        UT_Vector3R rot;
        bone->getRotates(rot.data());
        rot.degToRad();
        xform.prerotate(rot.x(), rot.y(), rot.z(), KIN_Chain::getXformOrder());

        // Stash the world transform's rotation into our orient attribute.
        UT_Matrix3R rot_xform(xform);
        UT_Quaternion q;
        q.updateFromRotationMatrix(rot_xform);
        ppt->setValue<UT_Quaternion>(orient_attrib, q);

        prev_length = bone->getLength();
    }
    // set chain end position
    xform.pretranslate(0, 0, -1 * prev_length);
    xform.getTranslates(pos);
    gdp->points()(num_bones)->setPos(pos);

    unlockInputs();
    return error();
}

// Provide input labels.
const char *
SOP_IKSample::inputLabel(unsigned /*input_index*/) const
{
    return "Points for IK";
}