Examples with DualQuaternion maspack.matrix.DualQuaternion used on opensource projects

Example 1 with DualQuaternion

use of maspack.matrix.DualQuaternion in project artisynth_core by artisynth.

the class PointSkinAttachment method computePosState.

/**
 * Computes this attachment's point value from all the underlying master
 * components to which it is connected.
 */
protected void computePosState(Vector3d pos, SkinMeshBody skinMesh) {
    DualQuaternion tmpQ = null;
    DualQuaternion[] dualqs = null;
    double[] weights = null;
    pos.setZero();
    Point3d tmp = new Point3d();
    // frame counter for interative dualQuaternion blending
    int fidx = 0;
    // weight for just the dual quaternion part
    double dualw = 0;
    for (int k = 0; k < myNumConnections; k++) {
        Connection c = myConnections[k];
        if (!c.accumulate(pos, tmp)) {
            // Go case-by-case when accumulate method is inactive
            if (c instanceof FrameConnection) {
                FrameInfo finfo = ((FrameConnection) c).myFrameInfo;
                switch(skinMesh.getFrameBlending()) {
                    case DUAL_QUATERNION_LINEAR:
                        {
                            if (tmpQ == null) {
                                tmpQ = new DualQuaternion();
                                tmpQ.scale(c.myWeight, finfo.myBlendQuaternion);
                            } else {
                                tmpQ.scaledAdd(c.myWeight, finfo.myBlendQuaternion);
                            }
                            dualw += c.myWeight;
                            break;
                        }
                    case DUAL_QUATERNION_ITERATIVE:
                        {
                            if (weights == null) {
                                int numf = skinMesh.numFrames();
                                weights = new double[numf];
                                dualqs = new DualQuaternion[numf];
                            }
                            dualqs[fidx] = finfo.myBlendQuaternion;
                            weights[fidx] = c.myWeight;
                            dualw += c.myWeight;
                            fidx++;
                            break;
                        }
                    default:
                        break;
                }
            }
        }
    }
    if (tmpQ != null) {
        tmpQ.normalize();
        tmpQ.transform(tmp, myBasePos);
        pos.scaledAdd(dualw, tmp);
    } else if (skinMesh.getFrameBlending() == FrameBlending.DUAL_QUATERNION_ITERATIVE && fidx > 0) {
        tmpQ = new DualQuaternion();
        tmpQ.dualQuaternionIterativeBlending(weights, dualqs, fidx, SkinMeshBody.DQ_BLEND_TOLERANCE, SkinMeshBody.DQ_MAX_BLEND_STEPS);
        tmpQ.transform(tmp, myBasePos);
        pos.scaledAdd(dualw, tmp);
    }
}