MQuaternion splineSolverNode::quaternionFromVectors(MVector vec1, MVector vec2)
{
MVector crosVec = vec1^vec2;
float w = sqrt((vec1.length() * vec1.length()) * (vec2.length() * vec2.length())) + vec1*vec2;
//float w = 1.0f + vBaseJoint*vFinalJoint;
MQuaternion q(crosVec.x, crosVec.y, crosVec.z, w );
q.normalizeIt();
return q;
}
// COMPUTE ======================================
MStatus gear_uToPercentage::compute(const MPlug& plug, MDataBlock& data)
{
MStatus returnStatus;
// Error check
if (plug != percentage)
return MS::kUnknownParameter;
// Curve
MFnNurbsCurve crv( data.inputValue( curve ).asNurbsCurve() );
// Sliders
bool in_normU = data.inputValue( normalizedU ).asBool();
double in_u = (double)data.inputValue( u ).asFloat();
unsigned in_steps = data.inputValue( steps ).asShort();
// Process
if (in_normU)
in_u = normalizedUToU(in_u, crv.numCVs());
// Get length
MVectorArray u_subpos(in_steps);
MVectorArray t_subpos(in_steps);
MPoint pt;
double step;
for (unsigned i = 0; i < in_steps ; i++){
step = i * in_u / (in_steps - 1.0);
crv.getPointAtParam(step, pt, MSpace::kWorld);
u_subpos[i] = MVector(pt);
step = i/(in_steps - 1.0);
crv.getPointAtParam(step, pt, MSpace::kWorld);
t_subpos[i] = MVector(pt);
}
double u_length = 0;
double t_length = 0;
MVector v;
for (unsigned i = 0; i < in_steps ; i++){
if (i>0){
v = u_subpos[i] - u_subpos[i-1];
u_length += v.length();
v = t_subpos[i] - t_subpos[i-1];
t_length += v.length();
}
}
double out_perc = (u_length / t_length) * 100;
// Output
MDataHandle h = data.outputValue( percentage );
h.setDouble( out_perc );
data.setClean( plug );
return MS::kSuccess;
}
MStatus VolumePushCollider::compute(const MPlug& plug, MDataBlock& dataBlock)
{
MStatus status;
if (plug == aOutput)
{
// inCollider
MArrayDataHandle hInCollider = dataBlock.inputArrayValue(aInCollider);
// inVolume
MArrayDataHandle hInVolume = dataBlock.inputArrayValue(aInVolume);
// output
MArrayDataHandle hOutput = dataBlock.inputArrayValue(aOutput);
MDoubleArray daValues(hInVolume.elementCount());
for (unsigned int c=0; c<hInCollider.elementCount(); c++)
{
// Calculate the total of every collider value for each volume
status = hInCollider.jumpToArrayElement(c);
CHECK_MSTATUS_AND_RETURN_IT(status);
MMatrix mInCollider = hInCollider.inputValue().asMatrix();
MTransformationMatrix tmInCollider(mInCollider);
MPoint pInCollider = tmInCollider.getTranslation(MSpace::kWorld, &status);
CHECK_MSTATUS_AND_RETURN_IT(status);
for (unsigned int v=0; v<hInVolume.elementCount(); v++)
{
// pointMatrixMult
status = hInVolume.jumpToArrayElement(v);
CHECK_MSTATUS_AND_RETURN_IT(status);
MMatrix mInVolume = hInVolume.inputValue().asMatrix();
MVector vVolCollider = pInCollider * mInVolume;
// condition
if (vVolCollider.length() <= 1.0)
{
// reverse
daValues[v] += abs(1.0 - vVolCollider.length());
}
}
}
for (unsigned int i=0; i<hInVolume.elementCount(); i++)
{
// set outputs
status = hOutput.jumpToArrayElement(i);
CHECK_MSTATUS_AND_RETURN_IT(status);
hOutput.outputValue().set(daValues[i]);
}
dataBlock.setClean(plug);
}
return MS::kSuccess;
}
void GetValidUp(const MDoubleArray& weights, const MPointArray& points,
const MIntArray& sampleIds, const MPoint& origin, const MVector& normal,
MVector& up) {
MVector unitUp = up.normal();
// Adjust up if it's parallel to normal or if it's zero length
if (std::abs((unitUp * normal) - 1.0) < 0.001 || up.length() < 0.0001) {
for (unsigned int j = 0; j < weights.length()-1; ++j) {
up -= (points[sampleIds[j]] - origin) * weights[j];
unitUp = up.normal();
if (std::abs((unitUp * normal) - 1.0) > 0.001 && up.length() > 0.0001) {
// If the up and normal vectors are no longer parallel and the up vector has a length,
// then we are good to go.
break;
}
}
up.normalize();
} else {
up = unitUp;
}
}
double GeometryMath::distanceToSegment(const MVector &segmentStart, const MVector &segmentEnd, const MVector &point,MVector *hitPoint){
MVector ab = segmentEnd-segmentStart;
MVector ap = point-segmentStart;
double dotBoundsTest = ap*ab.normal();
MVector projection = ab.normal()*dotBoundsTest;
MVector magnitude = ap-projection;
if (dotBoundsTest<=0) {
// return distance start--point
if (hitPoint)
*hitPoint = segmentStart;
return projection.length()+magnitude.length();
//return ap.length();
}
if (dotBoundsTest>=ab.length()){
// return distance end--point
if (hitPoint)
*hitPoint = segmentEnd;
return projection.length()+magnitude.length();
//MVector bp(point);
//bp -= segmentEnd;
//return bp.length();
}
if (hitPoint)
*hitPoint = segmentStart+projection;
return magnitude.length();
}
void CylinderMesh::transform(MPointArray& points, MVectorArray& normals)
{
MVector forward = mEnd - mStart;
double s = forward.length();
forward.normalize();
MVector left = MVector(0,0,1)^forward;
MVector up;
if (left.length() < 0.0001)
{
up = forward^MVector(0,1,0);
left = up^forward;
}
else
{
up = forward^left;
}
MMatrix mat;
mat[0][0] = forward[0]; mat[0][1] = left[0]; mat[0][2] = up[0]; mat[0][3] = 0;
mat[1][0] = forward[1]; mat[1][1] = left[1]; mat[1][2] = up[1]; mat[1][3] = 0;
mat[2][0] = forward[2]; mat[2][1] = left[2]; mat[2][2] = up[2]; mat[2][3] = 0;
mat[3][0] = 0; mat[3][1] = 0; mat[3][2] = 0; mat[3][3] = 1;
mat = mat.transpose();
for (int i = 0; i < gPoints.length(); i++)
{
MPoint p = gPoints[i];
p.x = p.x * s; // scale
p = p * mat + mStart; // transform
points.append(p);
MVector n = gNormals[i] * mat;
normals.append(n);
}
}
double MG_poseReader::projVector (MVector vec1,MVector vec2)
{
double proj;
float dot = vec1*vec2;
float vec2L = vec2.length();
if (vec2L!=0)
{
proj = dot/vec2L;
}else{
proj=0;
}
if ((proj<0) || (proj==-0))
proj=0;
return proj;
}
// used when ray does not intersect object
// to account for perspective, all edges are flattened onto
// a plane defined by the raySource and rayDirection
double gpuCacheIsectUtil::getEdgeSnapPointOnTriangle(const MPoint& raySource, const MVector& rayDirection, const MPoint& vert1, const MPoint& vert2, const MPoint& vert3, MPoint& snapPoint){
MPointArray verts;
verts.insert(vert1,0);
verts.insert(vert2,1);
verts.insert(vert3,2);
int edgeIndices[3][2]={{0,1},{1,2},{2,0}};
double minDistTri = std::numeric_limits<double>::max();
for(int edge=0; edge<3;edge++){
MPoint vertex1Org = verts[edgeIndices[edge][0]];
MPoint vertex2Org = verts[edgeIndices[edge][1]];
double coef_plane = rayDirection * raySource;
double d = coef_plane - rayDirection * vertex1Org;
MPoint vertex1 = vertex1Org + rayDirection * d;
d = coef_plane - rayDirection * vertex2Org;
MPoint vertex2 = vertex2Org + rayDirection * d;
MVector edgeDir = vertex2 - vertex1;
if (edgeDir.length()<0.0000001){
double dist = vertex1.distanceTo(raySource);
if (dist < minDistTri) {
minDistTri = dist;
snapPoint = vertex1Org;
}
} else {
MPoint edgePt;
// Compute the closest point from the edge to cursor Ray.
double percent = gpuCacheIsectUtil::getClosestPointOnLine(raySource, vertex1, vertex2, edgePt);
double dist = edgePt.distanceTo(raySource);
if (dist < minDistTri) {
minDistTri = dist;
snapPoint = (vertex1Org + percent * (vertex2Org - vertex1Org));
}
}
}
return minDistTri;
}
void MVGManipulator::getTranslatedWSEdgePoints(M3dView& view,
const MVGManipulatorCache::EdgeData* originEdgeData,
MPoint& originCSPosition, MPoint& targetWSPosition,
MPointArray& targetEdgeWSPositions) const
{
assert(originEdgeData != NULL);
MVector edgeCSVector =
MVGGeometryUtil::worldToCameraSpace(view, originEdgeData->vertex1->worldPosition) -
MVGGeometryUtil::worldToCameraSpace(view, originEdgeData->vertex2->worldPosition);
MVector vertex1ToMouseCSVector =
originCSPosition -
MVGGeometryUtil::worldToCameraSpace(view, originEdgeData->vertex1->worldPosition);
float ratioVertex1 = vertex1ToMouseCSVector.length() / edgeCSVector.length();
float ratioVertex2 = 1.f - ratioVertex1;
MVector edgeWSVector =
originEdgeData->vertex1->worldPosition - originEdgeData->vertex2->worldPosition;
targetEdgeWSPositions.append(targetWSPosition + ratioVertex1 * edgeWSVector);
targetEdgeWSPositions.append(targetWSPosition - ratioVertex2 * edgeWSVector);
}
MStatus MG_softIk::compute(const MPlug& plug,MDataBlock& dataBlock)
{
//Get data
if ((plug == outputTranslate) ||(plug == downScale) ||(plug == upScale) )
{
MMatrix startMatrixV = dataBlock.inputValue(startMatrix).asMatrix();
MMatrix endMatrixV = dataBlock.inputValue(endMatrix).asMatrix();
double upInitLengthV = dataBlock.inputValue(upInitLength).asDouble();
double downInitLengthV = dataBlock.inputValue(downInitLength).asDouble();
double softDistanceV = dataBlock.inputValue(softDistance).asDouble();
double stretchV= dataBlock.inputValue(stretch).asDouble();
double slideV = dataBlock.inputValue(slide).asDouble();
double globalScaleV = dataBlock.inputValue(globalScale).asDouble();
upInitLengthV = upInitLengthV*(slideV*2)*globalScaleV ;
downInitLengthV = downInitLengthV*((1 - slideV)*2)*globalScaleV;
double chainLength = upInitLengthV + downInitLengthV ;
MVector startVector (startMatrixV[3][0] , startMatrixV[3][1] , startMatrixV[3][2] );
MVector endVector (endMatrixV[3][0] , endMatrixV[3][1] , endMatrixV[3][2] );
MVector currentLengthVector = endVector - startVector ;
double currentLength = currentLengthVector.length() ;
double startSD = chainLength - softDistanceV;
double stretchParam = 1;
double upScaleOut = 1 ;
double downScaleOut = 1 ;
if (startSD <= currentLength)
{
if (softDistanceV != 0 )
{
double expParam = ( currentLength - startSD) / softDistanceV;
expParam*= -1 ;
double softLength = ( (softDistanceV *(1 - exp ( expParam)) )+ startSD );
currentLengthVector.normalize();
currentLengthVector*=softLength ;
if (stretchV != 0 )
{
stretchParam = (currentLength/softLength );
double delta = (stretchParam - 1);
delta*=stretchV;
stretchParam = 1*(1 + delta);
currentLengthVector *=(1 + delta );
}
}
}
//slide computing
upScaleOut = (slideV*2) *stretchParam ;
downScaleOut = ((1 - slideV)*2)*stretchParam;
MVector outVec = startVector + currentLengthVector;
dataBlock.outputValue(outputTranslate).setMVector(outVec);
dataBlock.outputValue(outputTranslate).setClean();
dataBlock.outputValue(downScale).set(downScaleOut);
dataBlock.outputValue(downScale).setClean();
dataBlock.outputValue(upScale).set(upScaleOut);
dataBlock.outputValue(upScale).setClean();
}
return MS::kSuccess;
}
MStatus CIKSolverNode::doSimpleSolver()
{
MStatus stat;
// Get the handle and create a function set for it
//
MIkHandleGroup* handle_group = handleGroup();
if (NULL == handle_group) {
return MS::kFailure;
}
MObject handle = handle_group->handle(0);
MDagPath handlePath = MDagPath::getAPathTo(handle);
MFnIkHandle fnHandle(handlePath, &stat);
// End-Effector
MDagPath endEffectorPath;
fnHandle.getEffector(endEffectorPath);
MFnIkEffector fnEffector(endEffectorPath);
MPoint effectorPos = fnEffector.rotatePivot(MSpace::kWorld);
unsigned int numJoints = endEffectorPath.length();
std::vector<MDagPath> jointsDagPaths; jointsDagPaths.reserve(numJoints);
while (endEffectorPath.length() > 1)
{
endEffectorPath.pop();
jointsDagPaths.push_back( endEffectorPath );
}
std::reverse(jointsDagPaths.begin(), jointsDagPaths.end());
static bool builtLocalSkeleton = false;
if (builtLocalSkeleton == false)
{
for (int jointIdx = 0; jointIdx < jointsDagPaths.size(); ++jointIdx)
{
MFnIkJoint curJoint(jointsDagPaths[jointIdx]);
m_localJointsPos.push_back( curJoint.getTranslation(MSpace::kWorld) );
}
m_localJointsPos.push_back(effectorPos );
builtLocalSkeleton = true;
}
MPoint startJointPos = MFnIkJoint(jointsDagPaths.front()).getTranslation(MSpace::kWorld);
MVector startToEndEff = m_localJointsPos.back() - m_localJointsPos.front();
double curveLength = (getPosition(1.0) - getPosition(0.0)).length();
double chainLength = startToEndEff.length(); // in local space.
double stretchFactor = curveLength / chainLength;
double uVal = 0.0f;
MVector jointPosL = m_localJointsPos[0];
for (int jointIdx = 0; jointIdx < jointsDagPaths.size(); ++jointIdx)
{
MFnIkJoint curJoint(jointsDagPaths[jointIdx]);
MVector curJointPosL = m_localJointsPos[jointIdx];
double dist = stretchFactor * (curJointPosL - jointPosL).length();
uVal = uVal + dist / curveLength;
MVector curCurveJointPos = getPosition(uVal);
curJoint.setTranslation(curCurveJointPos, MSpace::kWorld);
jointPosL = curJointPosL;
}
MVector effectorCurvePos = getPosition(1.0);
MVector curCurveJointPos = getPosition(uVal);
MVector effectorVec = (effectorCurvePos - curCurveJointPos).normal();
double endJointAngle[3];
MVector effectorVecXY = MVector(effectorVec(0), effectorVec(1), 0.0);
endJointAngle[2] = effectorVecXY.angle(MVector(1, 0, 0));
if ((MVector(1, 0, 0) ^ effectorVecXY) * MVector(0, 0, 1) < 0.0) { endJointAngle[2] = -endJointAngle[2]; }
MVector effectorVecXZ = MVector(effectorVec(0), 0.0, effectorVec(2));
endJointAngle[1] = effectorVecXZ.angle(MVector(1, 0, 0));
if ((MVector(1, 0, 0) ^ effectorVecXZ) * MVector(0, 1, 0) < 0.0) { endJointAngle[1] = -endJointAngle[1]; }
endJointAngle[0] = 0.0;
MFnIkJoint curJoint(jointsDagPaths.back()); curJoint.setRotation(endJointAngle, curJoint.rotationOrder());
return MS::kSuccess;
}
MStatus sgCurveEditBrush_context::editCurve( MDagPath dagPathCurve,
int beforeX, int beforeY, int currentX, int currentY, float radius,
const MDoubleArray& dArrLength, MPointArray &points )
{
MStatus status;
if( radius < 0 ) return MS::kSuccess;
MDagPath dagPathCam;
M3dView view = M3dView::active3dView( &status );
CHECK_MSTATUS_AND_RETURN_IT( status );
view.getCamera( dagPathCam );
MPoint camPos = dagPathCam.inclusiveMatrix()[3];
MVector vCamUp = dagPathCam.inclusiveMatrix()[1];
vCamUp.normalize();
radius *= .05;
MPoint nearClipBefore;
MPoint farClipBefore;
view.viewToWorld( beforeX, beforeY, nearClipBefore, farClipBefore );
MVector rayBefore = nearClipBefore - camPos;
rayBefore.normalize();
rayBefore *= 20;
MPoint posBefore = rayBefore + camPos;
MPoint nearClipCurrent;
MPoint farClipCurrent;
view.viewToWorld( currentX, currentY, nearClipCurrent, farClipCurrent );
MVector rayCurrent = nearClipCurrent - camPos;
rayCurrent.normalize();
rayCurrent *= 20;
MPoint posCurrent = rayCurrent + camPos;
MVector vMove = posCurrent - posBefore;
MMatrix mtxCurve = dagPathCurve.inclusiveMatrix();
MFnNurbsCurve fnCurve( dagPathCurve );
fnCurve.getCVs( points );
for( int i=0; i< points.length(); i++ )
{
points[i] *= mtxCurve;
}
for( int i=1; i< points.length(); i++ )
{
MPoint cuPoint = points[i];
MVector vPoint = cuPoint - camPos;
MVector projV = ( vPoint * rayBefore )/( pow( rayBefore.length(), 2 ) )* rayBefore;
MVector vertical = vPoint - projV;
float radiusForPoint = vertical.length() / projV.length();
if( radius < radiusForPoint )
continue;
MPoint parentPoint = points[i-1];
MVector vCurveDirection = cuPoint - parentPoint;
double vDirLength = vCurveDirection.length();
MVector vEditDirection = vCurveDirection + vMove/rayBefore.length()*projV.length();
double dotEdit = vCurveDirection.normal() * vEditDirection.normal();
if( dotEdit < 0 ) continue;
vEditDirection = vEditDirection * dotEdit + vCurveDirection*( 1-dotEdit );
MVector vEditLength = vEditDirection / vEditDirection.length() * vCurveDirection.length();
MVector vEdit = (vEditLength - vCurveDirection) * pow((double)(1-radiusForPoint/radius), 1 );
points[i] += vEdit;
for( int j=i+1; j< points.length(); j++ )
{
MPoint beforePoint = points[j];
MPoint pPoint = points[j-1];
MPoint beforePPoint = pPoint - vEdit;
MVector vBefore = points[j] - beforePPoint;
MVector vAfter = points[j] - pPoint;
MVector vCurrent = vAfter.normal() * dArrLength[j];
points[j] = vCurrent + pPoint;
vEdit = points[j] - beforePoint;
}
}
MMatrix invMtxCurve = mtxCurve.inverse();
for( int i=0; i< points.length(); i++ )
{
points[i] *= invMtxCurve;
}
fnCurve.setCVs( points );
fnCurve.updateCurve();
return MS::kSuccess;
}
void
simpleFluidEmitter::omniFluidEmitter(
MFnFluid& fluid,
const MMatrix& fluidWorldMatrix,
int plugIndex,
MDataBlock& block,
double dt,
double conversion,
double dropoff
)
//==============================================================================
//
// Method:
//
// simpleFluidEmitter::omniFluidEmitter
//
// Description:
//
// Emits fluid from a point, or from a set of object control points.
//
// Parameters:
//
// fluid: fluid into which we are emitting
// fluidWorldMatrix: object->world matrix for the fluid
// plugIndex: identifies which fluid connected to the emitter
// we are emitting into
// block: datablock for the emitter, to retrieve attribute
// values
// dt: time delta for this frame
// conversion: mapping from UI emission rates to internal units
// dropoff: specifies how much emission rate drops off as
// we move away from each emission point.
//
// Notes:
//
// If no owner object is present for the emitter, we simply emit from
// the emitter position. If an owner object is present, then we emit
// from each control point of that object in an identical fashion.
//
// To associate an owner object with an emitter, use the
// addDynamic MEL command, e.g. "addDynamic simpleFluidEmitter1 pPlane1".
//
//==============================================================================
{
// find the positions that we need to emit from
//
MVectorArray emitterPositions;
// first, try to get them from an owner object, which will have its
// "ownerPositionData" attribute feeding into the emitter. These
// values are in worldspace
//
bool gotOwnerPositions = false;
MObject ownerShape = getOwnerShape();
if( ownerShape != MObject::kNullObj )
{
MStatus status;
MDataHandle hOwnerPos = block.inputValue( mOwnerPosData, &status );
if( status == MS::kSuccess )
{
MObject dOwnerPos = hOwnerPos.data();
MFnVectorArrayData fnOwnerPos( dOwnerPos );
MVectorArray posArray = fnOwnerPos.array( &status );
if( status == MS::kSuccess )
{
// assign vectors from block to ownerPosArray.
//
for( unsigned int i = 0; i < posArray.length(); i ++ )
{
emitterPositions.append( posArray[i] );
}
gotOwnerPositions = true;
}
}
}
// there was no owner object, so we just use the emitter position for
// emission.
//
if( !gotOwnerPositions )
{
MPoint emitterPos = getWorldPosition();
emitterPositions.append( emitterPos );
}
// get emission rates for density, fuel, heat, and emission color
//
double densityEmit = fluidDensityEmission( block );
double fuelEmit = fluidFuelEmission( block );
double heatEmit = fluidHeatEmission( block );
bool doEmitColor = fluidEmitColor( block );
MColor emitColor = fluidColor( block );
// rate modulation based on frame time, user value conversion factor, and
// standard emitter "rate" value (not actually exposed in most fluid
// emitters, but there anyway).
//
double theRate = getRate(block) * dt * conversion;
// get voxel dimensions and sizes (object space)
//
double size[3];
unsigned int res[3];
fluid.getDimensions( size[0], size[1], size[2] );
fluid.getResolution( res[0], res[1], res[2] );
// voxel sizes
double dx = size[0] / res[0];
double dy = size[1] / res[1];
double dz = size[2] / res[2];
// voxel centers
double Ox = -size[0]/2;
double Oy = -size[1]/2;
double Oz = -size[2]/2;
// emission will only happen for voxels whose centers lie within
// "minDist" and "maxDist" of an emitter position
//
double minDist = getMinDistance( block );
double maxDist = getMaxDistance( block );
// bump up the min/max distance values so that they
// are both > 0, and there is at least about a half
// voxel between the min and max values, to prevent aliasing
// artifacts caused by emitters missing most voxel centers
//
MTransformationMatrix fluidXform( fluidWorldMatrix );
double fluidScale[3];
fluidXform.getScale( fluidScale, MSpace::kWorld );
// compute smallest voxel diagonal length
double wsX = fabs(fluidScale[0]*dx);
double wsY = fabs(fluidScale[1]*dy);
double wsZ = fabs(fluidScale[2]*dz);
double wsMin = MIN( MIN( wsX, wsY), wsZ );
double wsMax = MAX( MAX( wsX, wsY), wsZ );
double wsDiag = wsMin * sqrt(3.0);
// make sure emission range is bigger than 0.5 voxels
if ( maxDist <= minDist || maxDist <= (wsDiag/2.0) ) {
if ( minDist < 0 ) minDist = 0;
maxDist = minDist + wsDiag/2.0;
dropoff = 0;
}
// Now, it's time to actually emit into the fluid:
//
// foreach emitter point
// foreach voxel
// - select some points in the voxel
// - compute a dropoff function from the emitter point
// - emit an appropriate amount of fluid into the voxel
//
// Since we've already expanded the min/max distances to cover
// the smallest voxel dimension, we should only need 1 sample per
// voxel, unless the voxels are highly non-square. We increase the
// number of samples in these cases.
//
// If the "jitter" flag is enabled, we jitter each sample position,
// using the rangen() function, which keeps track of independent
// random states for each fluid, to make sure that results are
// repeatable for multiple simulation runs.
//
// basic sample count
int numSamples = 1;
// increase samples if necessary for non-square voxels
if(wsMin >.00001)
{
numSamples = (int)(wsMax/wsMin + .5);
if(numSamples > 8)
numSamples = 8;
if(numSamples < 1)
numSamples = 1;
}
bool jitter = fluidJitter(block);
if( !jitter )
{
// I don't have a good uniform sample generator for an
// arbitrary number of samples. It would be a good idea to use
// one here. For now, just use 1 sample for the non-jittered case.
//
numSamples = 1;
}
for( unsigned int p = 0; p < emitterPositions.length(); p++ )
{
MPoint emitterWorldPos = emitterPositions[p];
// loop through all voxels, looking for ones that lie at least
// partially within the dropoff field around this emitter point
//
for( unsigned int i = 0; i < res[0]; i++ )
{
double x = Ox + i*dx;
for( unsigned int j = 0; j < res[1]; j++ )
{
double y = Oy + j*dy;
for( unsigned int k = 0; k < res[2]; k++ )
{
double z = Oz + k*dz;
int si;
for( si = 0; si < numSamples; si++ )
{
// compute sample point (fluid object space)
//
double rx, ry, rz;
if( jitter )
{
rx = x + randgen()*dx;
ry = y + randgen()*dy;
rz = z + randgen()*dz;
}
else
{
rx = x + 0.5*dx;
ry = y + 0.5*dy;
rz = z + 0.5*dz;
}
// compute distance from sample to emitter point
//
MPoint point( rx, ry, rz );
point *= fluidWorldMatrix;
MVector diff = point - emitterWorldPos;
double distSquared = diff * diff;
double dist = diff.length();
// discard if outside min/max range
//
if( (dist < minDist) || (dist > maxDist) )
{
continue;
}
// drop off the emission rate according to the falloff
// parameter, and divide to accound for multiple samples
// in the voxel
//
double distDrop = dropoff * distSquared;
double newVal = theRate * exp( -distDrop ) / (double)numSamples;
// emit density/heat/fuel/color into the current voxel
//
if( newVal != 0 )
{
fluid.emitIntoArrays( (float) newVal, i, j, k, (float)densityEmit, (float)heatEmit, (float)fuelEmit, doEmitColor, emitColor );
}
float *fArray = fluid.falloff();
if( fArray != NULL )
{
MPoint midPoint( x+0.5*dx, y+0.5*dy, z+0.5*dz );
midPoint.x *= 0.2;
midPoint.y *= 0.2;
midPoint.z *= 0.2;
float fdist = (float) sqrt( midPoint.x*midPoint.x + midPoint.y*midPoint.y + midPoint.z*midPoint.z );
fdist /= sqrtf(3.0f);
fArray[fluid.index(i,j,k)] = 1.0f-fdist;
}
}
}
}
}
}
}
// used when ray does not intersect object
// to account for perspective, all edges are flattened onto
// a plane defined by the raySource and rayDirection
double gpuCacheIsectUtil::getEdgeSnapPointOnBox(const MPoint& raySource, const MVector& rayDirection, const MBoundingBox& bbox, MPoint& snapPoint){
//if ray intersects bbox
MPoint boxIntersectionPt;
if(firstRayIntersection(bbox.min(), bbox.max(), raySource, rayDirection, NULL, &boxIntersectionPt))
{
// ray intersects bounding box, so snapPoint is
// closest hit on the outside of the box
// and distance to box is 0 (for snapping purposes)
snapPoint = boxIntersectionPt;
return 0.0;
}
MPointArray verts;
MPoint vmin = bbox.min();
MPoint vmax = bbox.max();
verts.insert(vmin,0);
verts.insert(MPoint(vmax[0],vmin[1],vmin[2]),1);
verts.insert(MPoint(vmax[0],vmax[1],vmin[2]),2);
verts.insert(MPoint(vmin[0],vmax[1],vmin[2]),3);
verts.insert(MPoint(vmin[0],vmin[1],vmax[2]),4);
verts.insert(MPoint(vmax[0],vmin[1],vmax[2]),5);
verts.insert(vmax,6);
verts.insert(MPoint(vmin[0],vmax[1],vmax[2]),7);
int edgeIndices[12][2]={{0,1},{1,2},{2,3},{3,0},{4,5},{5,6},{6,7},{7,4},{0,4},{1,5},{3,7},{2,6}};
double minDistRect = std::numeric_limits<double>::max();
for(int edge=0; edge<12;edge++){
MPoint vertex1Org = verts[edgeIndices[edge][0]];
MPoint vertex2Org = verts[edgeIndices[edge][1]];
double coef_plane = rayDirection * raySource;
double d = coef_plane - rayDirection * vertex1Org;
MPoint vertex1 = vertex1Org + rayDirection * d;
d = coef_plane - rayDirection * vertex2Org;
MPoint vertex2 = vertex2Org + rayDirection * d;
MVector edgeDir = vertex2 - vertex1;
if (edgeDir.length()<0.0000001){
double dist = vertex1.distanceTo(raySource);
if (dist < minDistRect) {
minDistRect = dist;
snapPoint = vertex1Org;
}
} else {
MPoint edgePt;
// Compute the closest point from the edge to cursor Ray.
double percent = gpuCacheIsectUtil::getClosestPointOnLine(raySource, vertex1, vertex2, edgePt);
double dist = edgePt.distanceTo(raySource);
if (dist < minDistRect) {
minDistRect = dist;
snapPoint = (vertex1Org + percent * (vertex2Org - vertex1Org));
}
}
}
return minDistRect;
}
void gpuCacheIsectUtil::getClosestPointToRayOnLine(const MPoint& vertex1, const MPoint& vertex2, const MPoint& raySource, const MVector& rayDirection, MPoint& closestPoint, double& percent){
MPoint clsPoint;
MVector edgeDir = vertex2 - vertex1;
double len = edgeDir.length();
if(len < 0.0000001 )
{
percent = 0.0;
closestPoint = vertex1;
return;
}
edgeDir.normalize();
//if line is parallel to ray
double dotPrd = fabs(edgeDir * rayDirection);
if(dotPrd > 0.9999){
percent = 0.0;
closestPoint = vertex1;
return;
}
// Vector connecting two closest points.
//
MVector crossProd = edgeDir ^ rayDirection;
// Normal to the plane defined by that vector and the 'otherLine'.
//
MVector planeNormal = rayDirection ^ crossProd;
//intersectionPlane is raySource,planeNormal
double t;
if(intersectPlane(raySource, planeNormal, vertex1,edgeDir,t)){
clsPoint = vertex1 + t * edgeDir;
// Find percent, where
// vertex1 + percent * (edgeDir) == closestPoint
//
percent = edgeDir * (clsPoint - vertex1) / len;
// The closest point may not be on the segment. Find the closest
// point on the segment using t.
//
if (percent < 0)
{
closestPoint = vertex1;
percent = 0.0;
}
else if (percent > 1.0)
{
closestPoint = vertex2;
percent = 1.0;
}
else
{
closestPoint = clsPoint;
}
} else
{
closestPoint = vertex1;
percent = 0.0;
}
}
MStatus sgBulgeDeformer::deform(MDataBlock& dataBlock, MItGeometry& iter, const MMatrix& mtx, unsigned int index)
{
MStatus status;
float bulgeWeight = dataBlock.inputValue(aBulgeWeight).asFloat();
double bulgeRadius = dataBlock.inputValue(aBulgeRadius).asDouble();
MArrayDataHandle hArrInputs = dataBlock.inputArrayValue(aBulgeInputs);
MPointArray allPositions;
iter.allPositions(allPositions);
if (mem_resetElements)
{
unsigned int elementCount = hArrInputs.elementCount();
mem_meshInfosInner.resize(mem_maxLogicalIndex);
mem_meshInfosOuter.resize(mem_maxLogicalIndex);
for (unsigned int i = 0; i < elementCount; i++, hArrInputs.next())
{
MDataHandle hInput = hArrInputs.inputValue();
MDataHandle hMatrix = hInput.child(aMatrix);
MDataHandle hMesh = hInput.child(aMesh);
MMatrix mtxMesh = hMatrix.asMatrix();
MObject oMesh = hMesh.asMesh();
MFnMeshData meshDataInner, meshDataOuter;
MObject oMeshInner = meshDataInner.create();
MObject oMeshOuter = meshDataOuter.create();
MFnMesh fnMesh;
fnMesh.copy(oMesh, oMeshInner);
fnMesh.copy(oMesh, oMeshOuter);
sgMeshInfo* newMeshInfoInner = new sgMeshInfo(oMeshInner, hMatrix.asMatrix());
sgMeshInfo* newMeshInfoOuter = new sgMeshInfo(oMeshOuter, hMatrix.asMatrix());
mem_meshInfosInner[hArrInputs.elementIndex()] = newMeshInfoInner;
mem_meshInfosOuter[hArrInputs.elementIndex()] = newMeshInfoOuter;
}
}
for (unsigned int i = 0; i < elementCount; i++)
{
mem_meshInfosInner[i]->setBulge(bulgeWeight, MSpace::kWorld );
}
MFloatArray weightList;
weightList.setLength(allPositions.length());
for (unsigned int i = 0; i < weightList.length(); i++)
weightList[i] = 0.0f;
MMatrixArray inputMeshMatrixInverses;
inputMeshMatrixInverses.setLength(elementCount);
for (unsigned int i = 0; i < elementCount; i++)
{
inputMeshMatrixInverses[i] = mem_meshInfosInner[i]->matrix();
}
for (unsigned int i = 0; i < allPositions.length(); i++)
{
float resultWeight = 0;
for (unsigned int infoIndex = 0; infoIndex < elementCount; infoIndex++)
{
MPoint localPoint = allPositions[i] * mtx* inputMeshMatrixInverses[infoIndex];
MPoint innerPoint = mem_meshInfosInner[infoIndex]->getClosestPoint(localPoint);
MPoint outerPoint = mem_meshInfosOuter[infoIndex]->getClosestPoint(localPoint);
MVector innerVector = innerPoint - localPoint;
MVector outerVector = outerPoint - localPoint;
if (innerVector * outerVector < 0)
{
double innerLength = innerVector.length();
double outerLength = outerVector.length();
double allLength = innerLength + outerLength;
float numerator = float( innerLength * outerLength );
float denominator = float( pow(allLength / 2.0, 2) );
resultWeight = numerator / denominator;
}
}
weightList[i] = resultWeight;
}
for (unsigned int i = 0; i < allPositions.length(); i++)
{
allPositions[i] += weightList[i] * MVector(0, 1, 0);
}
iter.setAllPositions(allPositions);
return MS::kSuccess;
}
// COMPUTE ======================================
MStatus gear_rollSplineKine::compute(const MPlug& plug, MDataBlock& data)
{
MStatus returnStatus;
// Error check
if (plug != output)
return MS::kUnknownParameter;
// Get inputs matrices ------------------------------
// Inputs Parent
MArrayDataHandle adh = data.inputArrayValue( ctlParent );
int count = adh.elementCount();
if (count < 1)
return MS::kFailure;
MMatrixArray inputsP(count);
for (int i = 0 ; i < count ; i++){
adh.jumpToElement(i);
inputsP[i] = adh.inputValue().asMatrix();
}
// Inputs
adh = data.inputArrayValue( inputs );
if (count != adh.elementCount())
return MS::kFailure;
MMatrixArray inputs(count);
for (int i = 0 ; i < count ; i++){
adh.jumpToElement(i);
inputs[i] = adh.inputValue().asMatrix();
}
adh = data.inputArrayValue( inputsRoll );
if (count != adh.elementCount())
return MS::kFailure;
MDoubleArray roll(adh.elementCount());
for (int i = 0 ; i < count ; i++){
adh.jumpToElement(i);
roll[i] = degrees2radians((double)adh.inputValue().asFloat());
}
// Output Parent
MDataHandle ha = data.inputValue( outputParent );
MMatrix outputParent = ha.asMatrix();
// Get inputs sliders -------------------------------
double in_u = (double)data.inputValue( u ).asFloat();
bool in_resample = data.inputValue( resample ).asBool();
int in_subdiv = data.inputValue( subdiv ).asShort();
bool in_absolute = data.inputValue( absolute ).asBool();
// Process ------------------------------------------
// Get roll, pos, tan, rot, scl
MVectorArray pos(count);
MVectorArray tan(count);
MQuaternion *rot;
rot = new MQuaternion[count];
MVectorArray scl(count);
double threeDoubles[3];
for (int i = 0 ; i < count ; i++){
MTransformationMatrix tp(inputsP[i]);
MTransformationMatrix t(inputs[i]);
pos[i] = t.getTranslation(MSpace::kWorld);
rot[i] = tp.rotation();
t.getScale(threeDoubles, MSpace::kWorld);
scl[i] = MVector(threeDoubles[0], threeDoubles[1], threeDoubles[2]);
tan[i] = MVector(threeDoubles[0] * 2.5, 0, 0).rotateBy(t.rotation());
}
// Get step and indexes
// We define between wich controlers the object is to be able to
// calculate the bezier 4 points front this 2 objects
double step = 1.0 / max( 1, count-1.0 );
int index1 = (int)min( count-2.0, in_u/step );
int index2 = index1+1;
int index1temp = index1;
int index2temp = index2;
double v = (in_u - step * double(index1)) / step;
double vtemp = v;
// calculate the bezier
MVector bezierPos;
MVector xAxis, yAxis, zAxis;
if(!in_resample){
// straight bezier solve
MVectorArray results = bezier4point(pos[index1],tan[index1],pos[index2],tan[index2],v);
bezierPos = results[0];
xAxis = results[1];
}
else if(!in_absolute){
MVectorArray presample(in_subdiv);
MVectorArray presampletan(in_subdiv);
MDoubleArray samplelen(in_subdiv);
double samplestep = 1.0 / double(in_subdiv-1);
double sampleu = samplestep;
presample[0] = pos[index1];
presampletan[0] = tan[index1];
MVector prevsample(presample[0]);
MVector diff;
samplelen[0] = 0;
double overalllen = 0;
MVectorArray results(2);
for(long i=1;i<in_subdiv;i++,sampleu+=samplestep){
results = bezier4point(pos[index1],tan[index1],pos[index2],tan[index2],sampleu);
presample[i] = results[0];
presampletan[i] = results[1];
diff = presample[i] - prevsample;
overalllen += diff.length();
samplelen[i] = overalllen;
prevsample = presample[i];
}
// now as we have the
sampleu = 0;
for(long i=0;i<in_subdiv-1;i++,sampleu+=samplestep){
samplelen[i+1] = samplelen[i+1] / overalllen;
if(v>=samplelen[i] && v <= samplelen[i+1]){
v = (v - samplelen[i]) / (samplelen[i+1] - samplelen[i]);
bezierPos = linearInterpolate(presample[i],presample[i+1],v);
xAxis = linearInterpolate(presampletan[i],presampletan[i+1],v);
break;
}
}
}
else{
MVectorArray presample(in_subdiv);
MVectorArray presampletan(in_subdiv);
MDoubleArray samplelen(in_subdiv);
double samplestep = 1.0 / double(in_subdiv-1);
double sampleu = samplestep;
presample[0] = pos[0];
presampletan[0] = tan[0];
MVector prevsample(presample[0]);
MVector diff;
samplelen[0] = 0;
double overalllen = 0;
MVectorArray results;
for(long i=1;i<in_subdiv;i++,sampleu+=samplestep){
index1 = (int)min(count-2,sampleu / step);
index2 = index1+1;
v = (sampleu - step * double(index1)) / step;
results = bezier4point(pos[index1],tan[index1],pos[index2],tan[index2],v);
presample[i] = results[0];
presampletan[i] = results[1];
diff = presample[i] - prevsample;
overalllen += diff.length();
samplelen[i] = overalllen;
prevsample = presample[i];
}
// now as we have the
sampleu = 0;
for(long i=0;i<in_subdiv-1;i++,sampleu+=samplestep){
samplelen[i+1] = samplelen[i+1] / overalllen;
if(in_u>=samplelen[i] && in_u <= samplelen[i+1]){
in_u = (in_u - samplelen[i]) / (samplelen[i+1] - samplelen[i]);
bezierPos = linearInterpolate(presample[i],presample[i+1],in_u);
xAxis = linearInterpolate(presampletan[i],presampletan[i+1],in_u);
break;
}
}
}
// compute the scaling (straight interpolation!)
MVector scl1 = linearInterpolate(scl[index1temp], scl[index2temp],vtemp);
// compute the rotation!
MQuaternion q = slerp(rot[index1temp], rot[index2temp], vtemp);
yAxis = MVector(0,1,0);
yAxis = yAxis.rotateBy(q);
// use directly or project the roll values!
// print roll
double a = linearInterpolate(roll[index1temp], roll[index2temp], vtemp);
yAxis = yAxis.rotateBy( MQuaternion(xAxis.x * sin(a/2.0), xAxis.y * sin(a/2.0), xAxis.z * sin(a/2.0), cos(a/2.0)));
zAxis = xAxis ^ yAxis;
zAxis.normalize();
yAxis = zAxis ^ xAxis;
yAxis.normalize();
// Output -------------------------------------------
MTransformationMatrix result;
// translation
result.setTranslation(bezierPos, MSpace::kWorld);
// rotation
q = getQuaternionFromAxes(xAxis,yAxis,zAxis);
result.setRotationQuaternion(q.x, q.y, q.z, q.w);
// scaling
threeDoubles[0] = 1;
threeDoubles[0] = scl1.y;
threeDoubles[0] = scl1.z;
result.setScale(threeDoubles, MSpace::kWorld);
MDataHandle h = data.outputValue( output );
h.setMMatrix( result.asMatrix() * outputParent.inverse() );
data.setClean( plug );
return MS::kSuccess;
}
MStatus ik2Bsolver::doSolve()
//
// This is the doSolve method which calls solveIK.
//
{
MStatus stat;
// Handle Group
//
MIkHandleGroup * handle_group = handleGroup();
if (NULL == handle_group) {
return MS::kFailure;
}
// Handle
//
// For single chain types of solvers, get the 0th handle.
// Single chain solvers are solvers which act on one handle only,
// i.e. the handle group for a single chain solver
// has only one handle
//
MObject handle = handle_group->handle(0);
MDagPath handlePath = MDagPath::getAPathTo(handle);
MFnIkHandle handleFn(handlePath, &stat);
// Effector
//
MDagPath effectorPath;
handleFn.getEffector(effectorPath);
// MGlobal::displayInfo(effectorPath.fullPathName());
MFnIkEffector effectorFn(effectorPath);
// Mid Joint
//
effectorPath.pop();
MFnIkJoint midJointFn(effectorPath);
// End Joint
//
MDagPath endJointPath;
bool hasEndJ = findFirstJointChild(effectorPath, endJointPath);
// if(hasEndJ) MGlobal::displayInfo(endJointPath.fullPathName());
MFnIkJoint endJointFn(endJointPath);
// Start Joint
//
MDagPath startJointPath;
handleFn.getStartJoint(startJointPath);
MFnIkJoint startJointFn(startJointPath);
// Preferred angles
//
double startJointPrefAngle[3];
double midJointPrefAngle[3];
startJointFn.getPreferedAngle(startJointPrefAngle);
midJointFn.getPreferedAngle(midJointPrefAngle);
// Set to preferred angles
//
startJointFn.setRotation(startJointPrefAngle,
startJointFn.rotationOrder());
midJointFn.setRotation(midJointPrefAngle,
midJointFn.rotationOrder());
MPoint handlePos = handleFn.rotatePivot(MSpace::kWorld);
MPoint effectorPos = effectorFn.rotatePivot(MSpace::kWorld);
MPoint midJointPos = midJointFn.rotatePivot(MSpace::kWorld);
MPoint startJointPos = startJointFn.rotatePivot(MSpace::kWorld);
MVector poleVector = poleVectorFromHandle(handlePath);
poleVector *= handlePath.exclusiveMatrix();
double twistValue = twistFromHandle(handlePath);
MObject thisNode = thisMObject();
MVector localEndJointT = endJointFn.getTranslation(MSpace::kTransform);
MVector worldEndJointT = endJointFn.rotatePivot(MSpace::kWorld) - midJointPos;
double scaling = worldEndJointT.length() / ( localEndJointT.length() + 1e-6);
// MGlobal::displayInfo(MString(" scaling ")+scaling);
// get rest length
//
double restL1, restL2;
MPlug(thisNode, arestLength1).getValue(restL1);
MPlug(thisNode, arestLength2).getValue(restL2);
// get soft distance
//
MPlug plug( thisNode, asoftDistance );
double softD = 0.0;
plug.getValue(softD);
restL1 *= scaling;
restL2 *= scaling;
softD *= scaling;
// get max stretching
double maxStretching = 0.0;
MPlug(thisNode, amaxStretching).getValue(maxStretching);
MQuaternion qStart, qMid;
double stretching = 0.0;
solveIK(startJointPos,
midJointPos,
effectorPos,
handlePos,
poleVector,
twistValue,
qStart,
qMid,
softD,
restL1, restL2,
stretching);
midJointFn.rotateBy(qMid, MSpace::kWorld);
startJointFn.rotateBy(qStart, MSpace::kWorld);
midJointFn.setTranslation(MVector(restL1 / scaling, 0.0, 0.0), MSpace::kTransform);
endJointFn.setTranslation(MVector(restL2 / scaling, 0.0, 0.0), MSpace::kTransform);
// if(stretching > maxStretching) stretching = maxStretching;
if(maxStretching > 0.0) {
MVector vstretch(stretching* 0.5 / scaling, 0.0, 0.0);
midJointFn.translateBy(vstretch, MSpace::kTransform);
endJointFn.translateBy(vstretch, MSpace::kTransform);
}
return MS::kSuccess;
}
void ik2Bsolver::solveIK(const MPoint &startJointPos,
const MPoint &midJointPos,
const MPoint &effectorPos,
const MPoint &handlePos,
const MVector &poleVector,
double twistValue,
MQuaternion &qStart,
MQuaternion &qMid,
const double & softDistance,
const double & restLength1,
const double & restLength2,
double & stretching)
//
// This is method that actually computes the IK solution.
//
{
// vector from startJoint to midJoint
MVector vector1 = midJointPos - startJointPos; vector1 = vector1.normal() * restLength1;
// vector from midJoint to effector
MVector vector2 = effectorPos - midJointPos; vector2 = vector2.normal() * restLength2;
// vector from startJoint to handle
MVector vectorH = handlePos - startJointPos;
// vector from startJoint to effector
MVector vectorE = effectorPos - startJointPos;
// lengths of those vectors
const double length1 = restLength1;
const double length2 = restLength2;
const double lengthH = vectorH.length();
// component of the vector1 orthogonal to the vectorE
const MVector vectorO =
vector1 - vectorE*((vector1*vectorE)/(vectorE*vectorE));
//////////////////////////////////////////////////////////////////
// calculate q12 which solves for the midJoint rotation
//////////////////////////////////////////////////////////////////
// angle between vector1 and vector2
const double vectorAngle12 = vector1.angle(vector2);
// vector orthogonal to vector1 and 2
const MVector vectorCross12 = vector1^vector2;
const double lengthHsquared = lengthH * lengthH;
double weight = 0.0;
double slowLH = lengthH;// / (1.0 + (6.8 - softDistance) / (restLength1 + restLength2));
const double da = restLength1 + restLength2 - softDistance;
if(slowLH > da) {
float s = (slowLH - da) / softDistance;
if(s> 1.f) s= 1.f;
Vector3F pt = m_herm.interpolate(s);
// MGlobal::displayInfo(MString("herm ")+ pt.y);
// approading l1+l2 slower
//
weight = 1.0 - exp(-(slowLH - da) / softDistance * 6.98);
weight = pt.y * weight + (1.f - pt.y) * s;
slowLH = da + softDistance * weight;
// MGlobal::displayInfo(MString("wei ")+weight);
}
//
// 1
// / \
// l1 / \ l2
// / \ ---l1---1 ---l2---
// 0 ------ l ------ 2 0 ------ l ------- 2
//
// angle for arm extension
double cos_theta = (slowLH * slowLH - length1*length1 - length2*length2) / (2*length1*length2);
if (cos_theta > 1)
cos_theta = 1;
else if (cos_theta < -1)
cos_theta = -1;
const double theta = acos(cos_theta);
// quaternion for arm extension
MQuaternion q12(theta - vectorAngle12, vectorCross12);
//////////////////////////////////////////////////////////////////
// calculate qEH which solves for effector rotating onto the handle
//////////////////////////////////////////////////////////////////
// vector2 with quaternion q12 applied
vector2 = vector2.rotateBy(q12);
// vectorE with quaternion q12 applied
vectorE = vector1 + vector2;
// quaternion for rotating the effector onto the handle
MQuaternion qEH(vectorE, vectorH);
if(lengthH > vectorE.length()) {
// MGlobal::displayInfo(MString("vle ")+(lengthH-vectorE.length()));
stretching = (lengthH-vectorE.length()) * weight;
}
//////////////////////////////////////////////////////////////////
// calculate qNP which solves for the rotate plane
//////////////////////////////////////////////////////////////////
// vector1 with quaternion qEH applied
vector1 = vector1.rotateBy(qEH);
if (vector1.isParallel(vectorH))
// singular case, use orthogonal component instead
vector1 = vectorO.rotateBy(qEH);
// quaternion for rotate plane
MQuaternion qNP;
if (!poleVector.isParallel(vectorH) && (lengthHsquared != 0)) {
// component of vector1 orthogonal to vectorH
MVector vectorN =
vector1 - vectorH*((vector1*vectorH)/lengthHsquared);
// component of pole vector orthogonal to vectorH
MVector vectorP =
poleVector - vectorH*((poleVector*vectorH)/lengthHsquared);
double dotNP = (vectorN*vectorP)/(vectorN.length()*vectorP.length());
if (absoluteValue(dotNP + 1.0) < kEpsilon) {
// singular case, rotate halfway around vectorH
MQuaternion qNP1(kPi, vectorH);
qNP = qNP1;
}
else {
MQuaternion qNP2(vectorN, vectorP);
qNP = qNP2;
}
}
//////////////////////////////////////////////////////////////////
// calculate qTwist which adds the twist
//////////////////////////////////////////////////////////////////
MQuaternion qTwist(twistValue, vectorH);
// quaternion for the mid joint
qMid = q12;
// concatenate the quaternions for the start joint
qStart = qEH*qNP*qTwist;
}
// COMPUTE ======================================
MStatus gear_percentageToU::compute(const MPlug& plug, MDataBlock& data)
{
MStatus returnStatus;
// Error check
if (plug != percentage)
return MS::kUnknownParameter;
// Curve
MFnNurbsCurve crv( data.inputValue( curve ).asNurbsCurve() );
// Sliders
bool in_normU = data.inputValue( normalizedU ).asBool();
double in_percentage = (double)data.inputValue( percentage ).asFloat() * .01;
const unsigned in_steps = data.inputValue( steps ).asShort();
// Process
// Get length
MVectorArray u_subpos(in_steps);
MPoint pt;
MDoubleArray u_list(in_steps);
for(unsigned i = 0 ; i < in_steps ; i++ ){
u_list[i] = normalizedUToU(i /(in_steps - 1.0), crv.numCVs());
crv.getPointAtParam(u_list[i], pt, MSpace::kWorld);
u_subpos[i] = MVector(pt);
}
double t_length = 0;
MDoubleArray dist(in_steps);
MVector v;
for (unsigned i = 0; i < in_steps ; i++){
if (i>0){
v = u_subpos[i] - u_subpos[i-1];
t_length += v.length();
dist[i] = t_length;
}
}
MDoubleArray u_perc(in_steps);
for (unsigned i = 0; i < in_steps ; i++){
u_perc[i] = dist[i] / t_length;
}
// Get closest indices
unsigned index = findClosestInArray(in_percentage, u_perc);
unsigned indexA, indexB;
if (in_percentage <= u_perc[index]){
indexA = abs(int(index));
indexB = index;
if ( indexA > indexB){
indexA = indexB;
indexB = indexA+1;
}
}
else {
indexA = index;
indexB = index + 1;
}
// blend value
double blend = set01range(in_percentage, u_perc[indexA], u_perc[indexB]);
double out_u = linearInterpolate(u_list[indexA], u_list[indexB], blend);
if (in_normU)
out_u = uToNormalizedU(out_u, crv.numCVs());
// Ouput
MDataHandle h = data.outputValue( u );
h.setDouble( out_u );
data.setClean( plug );
return MS::kSuccess;
}
