Пример #1
0
void concludeContactCoulomb(const PxcSolverConstraintDesc& desc, PxcSolverContext& /*cache*/)
{
	PxU8* PX_RESTRICT cPtr = desc.constraint;

	const FloatV zero = FZero();

	const PxcSolverContactCoulombHeader* PX_RESTRICT firstHeader = (const PxcSolverContactCoulombHeader*)cPtr;
	PxU8* PX_RESTRICT last = desc.constraint + firstHeader->frictionOffset;//getConstraintLength(desc);
	while(cPtr < last)
	{
		const PxcSolverContactCoulombHeader* PX_RESTRICT hdr = (const PxcSolverContactCoulombHeader*)cPtr;
		cPtr += sizeof(PxcSolverContactCoulombHeader);

		const PxU32 numNormalConstr = hdr->numNormalConstr;
		
		//if(cPtr < last)
		//Ps::prefetchLine(cPtr, 512);
		Ps::prefetchLine(cPtr,128);
		Ps::prefetchLine(cPtr,256);
		Ps::prefetchLine(cPtr,384);

		const PxU32 pointStride = hdr->type == PXS_SC_TYPE_EXT_CONTACT ? sizeof(PxcSolverContactExt)
																	   : sizeof(PxcSolverContact);
		for(PxU32 i=0;i<numNormalConstr;i++)
		{
			PxcSolverContact *c = reinterpret_cast<PxcSolverContact*>(cPtr);
			cPtr += pointStride;
			c->setScaledBias(FMax(c->getScaledBias(), zero));
		}
	}
	PX_ASSERT(cPtr == last);
}
Пример #2
0
float GetSpeed(FVector projdirection, FVector PawnVelocity)
{
	FVector projvelocity = Scale(Normal(projdirection), 3920.0);
	projvelocity.Z += 0;

	float ForwardPct = FMin(Dot(Normal(PawnVelocity), Normal(projdirection)), 0.5);
	float InheritPct = FMax(0.5, ForwardPct);

	projvelocity.X += InheritPct * PawnVelocity.X;
	projvelocity.Y += InheritPct * PawnVelocity.Y;
	projvelocity.Z += 0.5 * PawnVelocity.Z;

	return VSize(projvelocity);
}
bool setupFinalizeExtSolverConstraintsCoulomb(PxcNpWorkUnit& n,
						    const ContactBuffer& buffer,
							const PxcCorrelationBufferCoulomb& c,
							const PxTransform& bodyFrame0,
							const PxTransform& bodyFrame1,
							bool /*perPointFriction*/,
							PxU8* workspace,
							PxReal invDt,
							PxReal bounceThreshold,
							PxsSolverExtBody& b0,
							PxsSolverExtBody& b1,
							PxU32 frictionCountPerPoint,
							PxReal invMassScale0, PxReal invInertiaScale0, 
							PxReal invMassScale1, PxReal invInertiaScale1)	
{
	// NOTE II: the friction patches are sparse (some of them have no contact patches, and
	// therefore did not get written back to the cache) but the patch addresses are dense,
	// corresponding to valid patches

	PxU8* PX_RESTRICT ptr = workspace;
	const FloatV zero=FZero();

	//KS - TODO - this should all be done in SIMD to avoid LHS
	const PxF32 maxPenBias0 = b0.mLinkIndex == PxcSolverConstraintDesc::NO_LINK ? b0.mBodyData->penBiasClamp : getMaxPenBias(*b0.mFsData)[b0.mLinkIndex];
	const PxF32 maxPenBias1 = b1.mLinkIndex == PxcSolverConstraintDesc::NO_LINK ? b1.mBodyData->penBiasClamp : getMaxPenBias(*b1.mFsData)[b0.mLinkIndex];

	const FloatV maxPen = FLoad(PxMax(maxPenBias0, maxPenBias1)/invDt);

	const FloatV restDistance = FLoad(n.restDistance); 

	Ps::prefetchLine(c.contactID);
	Ps::prefetchLine(c.contactID, 128);

	bool useExtContacts = (n.flags & (PxcNpWorkUnitFlag::eARTICULATION_BODY0|PxcNpWorkUnitFlag::eARTICULATION_BODY1))!=0;

	const PxU32 frictionPatchCount = c.frictionPatchCount;
	const bool staticBody = ((n.flags & PxcNpWorkUnitFlag::eDYNAMIC_BODY1) == 0);

	const PxU32 pointStride = useExtContacts ? sizeof(PxcSolverContactExt) : sizeof(PxcSolverContact);
	const PxU32 frictionStride = useExtContacts ? sizeof(PxcSolverFrictionExt) : sizeof(PxcSolverFriction);
	const PxU8 pointHeaderType = Ps::to8(useExtContacts ? PXS_SC_TYPE_EXT_CONTACT : (staticBody ? PXS_SC_TYPE_STATIC_CONTACT : PXS_SC_TYPE_RB_CONTACT));
	const PxU8 frictionHeaderType = Ps::to8(useExtContacts ? PXS_SC_TYPE_EXT_FRICTION : (staticBody ? PXS_SC_TYPE_STATIC_FRICTION : PXS_SC_TYPE_FRICTION));

	PxReal d0 = n.dominance0 * invMassScale0;
	PxReal d1 = n.dominance1 * invMassScale1;
	PxReal angD0 = n.dominance0 * invInertiaScale0;
	PxReal angD1 = n.dominance1 * invInertiaScale1;

	for(PxU32 i=0;i< frictionPatchCount;i++)
	{
		const PxU32 contactCount = c.frictionPatchContactCounts[i];
		if(contactCount == 0)
			continue;

		const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;

		const PxcFrictionPatchCoulomb& frictionPatch = c.frictionPatches[i];

		const Vec3V normalV = Ps::aos::V3LoadU(frictionPatch.normal);
		const PxVec3 normal = frictionPatch.normal;

		const PxReal combinedRestitution = contactBase0->restitution;
	
		
		PxcSolverContactCoulombHeader* PX_RESTRICT header = reinterpret_cast<PxcSolverContactCoulombHeader*>(ptr);
		ptr += sizeof(PxcSolverContactCoulombHeader);

		Ps::prefetchLine(ptr, 128);
		Ps::prefetchLine(ptr, 256);
		Ps::prefetchLine(ptr, 384);


		header->numNormalConstr		= (PxU8)contactCount;
		header->type				= pointHeaderType;
		header->setRestitution(combinedRestitution);

		header->setDominance0(d0);
		header->setDominance1(d1);
		header->angDom0 = angD0;
		header->angDom1 = angD1;
		
		header->setNormal(normalV);
		
		for(PxU32 patch=c.correlationListHeads[i]; 
			patch!=PxcCorrelationBuffer::LIST_END; 
			patch = c.contactPatches[patch].next)
		{
			const PxU32 count = c.contactPatches[patch].count;
			const Gu::ContactPoint* contactBase = buffer.contacts + c.contactPatches[patch].start;
				
			PxU8* p = ptr;
			for(PxU32 j=0;j<count;j++)
			{
				const Gu::ContactPoint& contact = contactBase[j];

				PxcSolverContactExt* PX_RESTRICT solverContact = reinterpret_cast<PxcSolverContactExt*>(p);
				p += pointStride;

				const FloatV separation = FLoad(contact.separation);

				PxVec3 ra = contact.point - bodyFrame0.p; 
				PxVec3 rb = contact.point - bodyFrame1.p; 

				Vec3V targetVel = V3LoadU(contact.targetVel);
				const FloatV maxImpulse = FLoad(contact.maxImpulse);

				solverContact->scaledBiasX_targetVelocityY_maxImpulseZ = V3Merge(FMax(maxPen, FSub(separation, restDistance)), V3Dot(normalV,targetVel), maxImpulse);

				//TODO - should we do cross only in vector land and then store. Could cause a LHS but probably no worse than
				//what we already have (probably has a LHS from converting from vector to scalar above)
				const PxVec3 raXn = ra.cross(normal);
				const PxVec3 rbXn = rb.cross(normal);

				Cm::SpatialVector deltaV0, deltaV1;

				PxReal unitResponse = getImpulseResponse(b0, Cm::SpatialVector(normal, raXn), deltaV0, d0, angD0,
														 b1, Cm::SpatialVector(-normal, -rbXn), deltaV1, d1, angD1);

				const PxReal vrel = b0.projectVelocity(normal, raXn)
								  - b1.projectVelocity(normal, rbXn);

				solverContact->raXnXYZ_appliedForceW = V4SetW(Vec4V_From_Vec3V(V3LoadU(raXn)), zero);
				solverContact->rbXnXYZ_velMultiplierW = V4SetW(Vec4V_From_Vec3V(V3LoadU(rbXn)), zero);

				completeContactPoint(*solverContact, unitResponse, vrel, invDt, header->restitution, bounceThreshold);

				solverContact->setDeltaVA(deltaV0.linear, deltaV0.angular);
				solverContact->setDeltaVB(deltaV1.linear, deltaV1.angular);


			}			
			ptr = p;
		}
	}

	//construct all the frictions

	PxU8* PX_RESTRICT ptr2 = workspace;

	const PxF32 orthoThreshold = 0.70710678f;
	const PxF32 eps = 0.00001f;
	bool hasFriction = false;

	for(PxU32 i=0;i< frictionPatchCount;i++)
	{
		const PxU32 contactCount = c.frictionPatchContactCounts[i];
		if(contactCount == 0)
			continue;

		PxcSolverContactCoulombHeader* header = reinterpret_cast<PxcSolverContactCoulombHeader*>(ptr2); 
		header->frictionOffset = PxU16(ptr - ptr2);
		ptr2 += sizeof(PxcSolverContactCoulombHeader) + header->numNormalConstr * pointStride;

		PxVec3 normal = c.frictionPatches[i].normal;

		const Gu::ContactPoint* contactBase0 = buffer.contacts + c.contactPatches[c.correlationListHeads[i]].start;

		const PxReal staticFriction = contactBase0->staticFriction;
		const PxU32 disableStrongFriction = contactBase0->internalFaceIndex1 & PxMaterialFlag::eDISABLE_FRICTION;
		const bool haveFriction = (disableStrongFriction == 0);
	
		PxcSolverFrictionHeader* frictionHeader = (PxcSolverFrictionHeader*)ptr;
		frictionHeader->numNormalConstr = Ps::to8(c.frictionPatchContactCounts[i]);
		frictionHeader->numFrictionConstr = Ps::to8(haveFriction ? c.frictionPatches[i].numConstraints : 0);
		ptr += sizeof(PxcSolverFrictionHeader);
		ptr += frictionHeader->getAppliedForcePaddingSize(c.frictionPatchContactCounts[i]);
		Ps::prefetchLine(ptr, 128);
		Ps::prefetchLine(ptr, 256);
		Ps::prefetchLine(ptr, 384);


		const PxVec3 t0Fallback1(0.f, -normal.z, normal.y);
		const PxVec3 t0Fallback2(-normal.y, normal.x, 0.f) ;
		const PxVec3 tFallback1 = orthoThreshold > PxAbs(normal.x) ? t0Fallback1 : t0Fallback2;
		const PxVec3 vrel = b0.getLinVel() - b1.getLinVel();
		const PxVec3 t0_ = vrel - normal * (normal.dot(vrel));
		const PxReal sqDist = t0_.dot(t0_);
		const PxVec3 tDir0 = (sqDist > eps ? t0_: tFallback1).getNormalized();
		const PxVec3 tDir1 = tDir0.cross(normal);
		PxVec3 tFallback[2] = {tDir0, tDir1};

		PxU32 ind = 0;

		if(haveFriction)
		{
			hasFriction = true;
			frictionHeader->setStaticFriction(staticFriction);
			frictionHeader->setDominance0(n.dominance0);
			frictionHeader->setDominance1(n.dominance1);
			frictionHeader->angDom0 = angD0;
			frictionHeader->angDom1 = angD1;
			frictionHeader->type			= frictionHeaderType;
			
			PxU32 totalPatchContactCount = 0;
		
			for(PxU32 patch=c.correlationListHeads[i]; 
				patch!=PxcCorrelationBuffer::LIST_END; 
				patch = c.contactPatches[patch].next)
			{
				const PxU32 count = c.contactPatches[patch].count;
				const PxU32 start = c.contactPatches[patch].start;
				const Gu::ContactPoint* contactBase = buffer.contacts + start;
					
				PxU8* p = ptr;

				

				for(PxU32 j =0; j < count; j++)
				{
					const PxU32 contactId = totalPatchContactCount + j;
					const Gu::ContactPoint& contact = contactBase[j];
					const PxVec3 ra = contact.point - bodyFrame0.p;
					const PxVec3 rb = contact.point - bodyFrame1.p;
					
					for(PxU32 k = 0; k < frictionCountPerPoint; ++k)
					{
						PxcSolverFrictionExt* PX_RESTRICT f0 = reinterpret_cast<PxcSolverFrictionExt*>(p);
						p += frictionStride;
						f0->contactIndex = contactId;

						PxVec3 t0 = tFallback[ind];
						ind = 1 - ind;
						PxVec3 raXn = ra.cross(t0); 
						PxVec3 rbXn = rb.cross(t0); 
						Cm::SpatialVector deltaV0, deltaV1;
						PxReal unitResponse = getImpulseResponse(b0, Cm::SpatialVector(t0, raXn), deltaV0, d0, angD0,
																 b1, Cm::SpatialVector(-t0, -rbXn), deltaV1, d1, angD1);

						f0->setVelMultiplier(FLoad(unitResponse>0.0f ? 1.f/unitResponse : 0.0f));
						f0->setRaXn(raXn);
						f0->setRbXn(rbXn);
						f0->setNormal(t0);
						f0->setAppliedForce(0.0f);
						f0->setDeltaVA(deltaV0.linear, deltaV0.angular);
						f0->setDeltaVB(deltaV1.linear, deltaV1.angular);
					}					
				}

				totalPatchContactCount += c.contactPatches[patch].count;
				
				ptr = p;	
			}
		}
	}
	//PX_ASSERT(ptr - workspace == n.solverConstraintSize);
	return hasFriction;
}
Пример #4
0
Vector SDKGradientClass::Output(BaseShader *sh, ChannelData *sd)
{
	Vector p=sd->p;
	Real	 r=0.0,angle,xx,yy;

	if (gdata.turbulence>0.0)
	{
		Vector	res;
		Real		scl=5.0*gdata.scale,tt=sd->t*gdata.freq*0.3;

		res = Vector(Turbulence(p*scl,tt,gdata.octaves,TRUE),Turbulence((p+Vector(0.34,13.0,2.43))*scl,tt,gdata.octaves,TRUE),0.0);

		if (gdata.absolute)
		{
			p.x  = Mix(p.x,res.x,gdata.turbulence);
			p.y  = Mix(p.y,res.y,gdata.turbulence);
		}
		else
		{
			p.x += (res.x-0.5)*gdata.turbulence;
			p.y += (res.y-0.5)*gdata.turbulence;
		}
	}

	// rotation
	p.x -= 0.5;
	p.y -= 0.5;

	xx = gdata.ca*p.x-gdata.sa*p.y + 0.5;
	yy = gdata.sa*p.x+gdata.ca*p.y + 0.5;

	p.x = xx;
	p.y = yy;

	if (gdata.mode<=SDKGRADIENTSHADER_MODE_CORNER && gdata.cycle && (sd->texflag&TEX_TILE))
	{
		if (sd->texflag & TEX_MIRROR)
		{
			p.x = Modulo(p.x,RCO 2.0);
			if (p.x>=1.0) p.x=2.0-p.x;

			p.y = Modulo(p.y,RCO 2.0);
			if (p.y>= 1.0) p.y=2.0-p.y;
		}
		else
		{
			p.x = Modulo(p.x, RCO 1.0);
			p.y = Modulo(p.y, RCO 1.0);
		}
	}

	switch (gdata.mode)
	{
		case SDKGRADIENTSHADER_MODE_U:
			r = p.x; 
			break;

		case SDKGRADIENTSHADER_MODE_V:
			r = 1.0-p.y; 
			break;

		case SDKGRADIENTSHADER_MODE_DIAGONAL:
			r = (p.x+p.y)*0.5; 
			break;

		case SDKGRADIENTSHADER_MODE_RADIAL:
			p.x-=0.5;
			p.y-=0.5;
			if (p.x==0.0) p.x=0.00001;

			angle = ATan(p.y/p.x);
			if (p.x<0.0) angle+=pi;
			if (angle<0.0) angle+=pi2;
			r = angle/pi2;
			break;

		case SDKGRADIENTSHADER_MODE_CIRCULAR:
			p.x-=0.5;
			p.y-=0.5;
			r = Sqrt(p.x*p.x+p.y*p.y)*2.0;
			break;

		case SDKGRADIENTSHADER_MODE_BOX:
			p.x = Abs(p.x - 0.5);
			p.y = Abs(p.y - 0.5);
			r   = FMax(p.x,p.y)*2.0;
			break;

		case SDKGRADIENTSHADER_MODE_STAR:
			p.x = Abs(p.x - 0.5)-0.5;
			p.y = Abs(p.y - 0.5)-0.5;
			r   = Sqrt(p.x*p.x+p.y*p.y) * 1.4142;
			break;

		case SDKGRADIENTSHADER_MODE_CORNER:
		{
			Real		cx;
			Vector	ca,cb;

			cx = FCut01(p.x);
			ca = Mix(gdata.c[0],gdata.c[1],cx);
			cb = Mix(gdata.c[2],gdata.c[3],cx);

			return Mix(ca,cb,FCut01(p.y));
			break;
		}
	}

	return gdata.gradient->CalcGradientPixel(FCut01(r));
}
Пример #5
0
void solveContactCoulomb_BStatic(const PxcSolverConstraintDesc& desc, PxcSolverContext& /*cache*/)
{
	PxcSolverBody& b0 = *desc.bodyA;


	Vec3V linVel0 = V3LoadA(b0.linearVelocity);
	Vec3V angVel0 = V3LoadA(b0.angularVelocity);

	PxcSolverContactCoulombHeader* firstHeader = (PxcSolverContactCoulombHeader*)desc.constraint;
	const PxU8* PX_RESTRICT last = desc.constraint + firstHeader->frictionOffset;//getConstraintLength(desc);

	//hopefully pointer aliasing doesn't bite.
	const PxU8* PX_RESTRICT currPtr = desc.constraint;

	const FloatV zero = FZero();

	while(currPtr < last)
	{
		PxcSolverContactCoulombHeader* PX_RESTRICT hdr = (PxcSolverContactCoulombHeader*)currPtr;
		currPtr += sizeof(PxcSolverContactCoulombHeader);

		const PxU32 numNormalConstr = hdr->numNormalConstr;

		PxcSolverContact* PX_RESTRICT contacts = (PxcSolverContact*)currPtr;
		Ps::prefetchLine(contacts);
		currPtr += numNormalConstr * sizeof(PxcSolverContact);

		PxF32* appliedImpulse = (PxF32*) (((PxU8*)hdr) + hdr->frictionOffset + sizeof(PxcSolverFrictionHeader));
		Ps::prefetchLine(appliedImpulse);

		const Vec3V normal = hdr->getNormal();

		const FloatV invMassDom0 = FLoad(hdr->dominance0);

		FloatV normalVel1 = V3Dot(normal, linVel0);

		const Vec3V delLinVel0 = V3Scale(normal, invMassDom0);
		FloatV accumDeltaF = zero;
		//FloatV accumImpulse = zero;

		for(PxU32 i=0;i<numNormalConstr;i++)
		{
			PxcSolverContact& c = contacts[i];
			Ps::prefetchLine(&contacts[i+1]);

			//const Vec4V normalXYZ_velMultiplierW = c.normalXYZ_velMultiplierW;
			const Vec4V raXnXYZ_appliedForceW = c.raXnXYZ_appliedForceW;
			const Vec4V rbXnXYZ_velMultiplierW = c.rbXnXYZ_velMultiplierW;

			//const Vec3V normal = c.normal;
			//const Vec3V normal = Vec3V_From_Vec4V(normalXYZ_velMultiplierW);
			const Vec3V raXn = Vec3V_From_Vec4V(raXnXYZ_appliedForceW);

			const FloatV appliedForce = V4GetW(raXnXYZ_appliedForceW);
			const FloatV velMultiplier = V4GetW(rbXnXYZ_velMultiplierW);
			//const FloatV velMultiplier = V4GetW(normalXYZ_velMultiplierW);

			const Vec3V delAngVel0 = Vec3V_From_Vec4V(c.delAngVel0_InvMassADom);

			const FloatV targetVel = c.getTargetVelocity();
			const FloatV nScaledBias = FNeg(c.getScaledBias());
			const FloatV maxImpulse = c.getMaxImpulse();

			//Compute the normal velocity of the constraint.

			//const FloatV normalVel1 = V3Dot(normal, linVel0);
			const FloatV normalVel2 = V3Dot(raXn, angVel0);
			const FloatV normalVel =  FAdd(normalVel1, normalVel2);

			//const FloatV unbiasedErr = FMul(targetVel, velMultiplier);
			const FloatV biasedErr = FMulAdd(targetVel, velMultiplier, nScaledBias);

			// still lots to do here: using loop pipelining we can interweave this code with the
			// above - the code here has a lot of stalls that we would thereby eliminate

			const FloatV _deltaF = FMax(FNegMulSub(normalVel, velMultiplier, biasedErr), FNeg(appliedForce));
			const FloatV _newForce = FAdd(appliedForce, _deltaF);
			const FloatV newForce = FMin(_newForce, maxImpulse);
			const FloatV deltaF = FSub(newForce, appliedForce);

			//linVel0 = V3MulAdd(delLinVel0, deltaF, linVel0);
			normalVel1 = FScaleAdd(invMassDom0, deltaF, normalVel1);
			angVel0 = V3ScaleAdd(delAngVel0, deltaF, angVel0);

			accumDeltaF = FAdd(accumDeltaF, deltaF);

			c.setAppliedForce(newForce);
			Ps::aos::FStore(newForce, &appliedImpulse[i]);
			Ps::prefetchLine(&appliedImpulse[i], 128);

			//accumImpulse = FAdd(accumImpulse, newAppliedForce);
		}
		linVel0 = V3ScaleAdd(delLinVel0, accumDeltaF, linVel0);
		//hdr->setAccumlatedForce(accumImpulse);
	}

	// Write back
	V3StoreU(linVel0, b0.linearVelocity);
	V3StoreU(angVel0, b0.angularVelocity);

	PX_ASSERT(currPtr == last);
}
Пример #6
0
/* Function: EmitConsensusSequence()
 * Date:     SRE, Wed Nov 11 11:08:59 1998 [St. Louis]
 *
 * Purpose:  Generate a "consensus sequence". For the purposes
 *           of a profile HMM, this is defined as:
 *              - for each node:
 *                 - if StateOccupancy() says that M is used 
 *                     with probability >= 0.5, this M is "consensus".
 *                     Then, choose maximally likely residue.
 *                     if P>0.5 (protein) or P>0.9 (DNA), make
 *                     it upper case; else make it lower case. 
 *                 - if StateOccupancy() says that I
 *                     is used with P >= 0.5, this I is "consensus";
 *                     use it 1/(1-TII) times (its expectation value).
 *                     Generate an "x" from each I.
 *                     
 *           The function expects that the model is config'ed
 *           by Plan7NakedConfig(): that is, for a single global pass
 *           with no N,C,J involvement.
 *                     
 *
 * Args:     hmm     - the model
 *           ret_seq - RETURN: consensus sequence (pass NULL if unwanted)
 *           ret_dsq - RETURN: digitized consensus sequence (pass NULL if unwanted)
 *           ret_L   - RETURN: length of generated sequence 
 *           ret_tr  - RETURN: generated trace (pass NULL if unwanted)
 *
 * Returns:  void        
 */
void
EmitConsensusSequence(struct plan7_s *hmm, char **ret_seq, char **ret_dsq, int *ret_L, struct p7trace_s **ret_tr)
{
  struct p7trace_s *tr;         /* RETURN: traceback */
  char *dsq, *seq;              /* sequence in digitized and undigitized form */
  float *mp, *ip, *dp;          /* state occupancies from StateOccupancy() */
  int    nmat, ndel, nins;	/* number of matches, deletes, inserts used */
  int    k;			/* counter for nodes */
  int    tpos;			/* position in trace */
  int    i;                     /* position in seq (equiv pos in dsq is i+1 */
  int    x;			/* symbol choice (M) or # symbols (I) */
  float  mthresh;		/* >= this, show symbol as upper case */

  if (Alphabet_type == hmmAMINO) mthresh = 0.5;
  else                           mthresh = 0.9;

  StateOccupancy(hmm, &mp, &ip, &dp);

  /* First pass: how many states do we need in the trace?
   *             how long will the sequence be?
   */
  nmat = ndel = nins = 0;
  for (k = 1; k <= hmm->M; k++)
    {
      if (mp[k] >= 0.5) nmat++; else ndel++;
      if (k < hmm->M && ip[k] >= 0.5) 
	nins += (int) (1.f / (1.f - hmm->t[k][TII]));
    }
  
  /* Allocations
   */
  P7AllocTrace(6 + nmat + ndel + nins, &tr);
  dsq = MallocOrDie(sizeof(char) * (nmat+nins+3));
  seq = MallocOrDie(sizeof(char) * (nmat+nins+1));

  /* Main pass.
   * Construct consensus trace, seq, and dsq.
   */
  TraceSet(tr, 0, STS, 0, 0);
  TraceSet(tr, 1, STN, 0, 0);
  TraceSet(tr, 2, STB, 0, 0);
  dsq[0] = Alphabet_iupac;	/* guard byte */
  tpos = 3;
  i    = 0;
  for (k = 1; k <= hmm->M; k++)
    {
      if (mp[k] >= 0.5)
	{
	  x = FMax(hmm->mat[k], Alphabet_size);
	  TraceSet(tr, tpos, STM, k, i+1);
	  seq[i]   = Alphabet[x];
	  dsq[i+1] = x;
	  if (hmm->mat[k][x] < mthresh)
	    seq[i] = tolower((int) seq[i]);
	  i++;
	  tpos++;
	}
      else
	{
	  TraceSet(tr, tpos, STD, k, 0);
	  tpos++;
	}

      if (k < hmm->M && ip[k] >= 0.5)
	{
	  x = (int) (1.f / (1.f - hmm->t[k][TII]));
	  while (x--) 
	    {
	      TraceSet(tr, tpos, STI, k, i+1);
	      seq[i]   = 'x';
	      dsq[i+1] = Alphabet_iupac - 1;
	      i++; 
	      tpos++;
	    }
	}
    }
  TraceSet(tr, tpos, STE, 0, 0); tpos++;
  TraceSet(tr, tpos, STC, 0, 0); tpos++;
  TraceSet(tr, tpos, STT, 0, 0); tpos++;
  dsq[i+1] = Alphabet_iupac;
    
  free(mp);
  free(ip);
  free(dp);
  if (ret_seq != NULL) *ret_seq = seq; else free(seq);
  if (ret_dsq != NULL) *ret_dsq = dsq; else free(dsq);
  if (ret_L   != NULL) *ret_L   = i;   
  if (ret_tr  != NULL) *ret_tr  = tr;  else P7FreeTrace(tr);
}
Пример #7
0
/* Function: EmitBestSequence()
 * Date:     SRE, Tue Nov 10 16:21:59 1998 [St. Louis]
 *
 * Purpose:  Given a model, emit the maximum probability sequence
 *           from it: argmax_{seq} P(seq | model).
 *           This is a sensible HMM equivalent to a "consensus"
 *           sequence.
 *           The model should be Plan7NakedConfig()'ed; 
 *           in particular, if we allowed B->M and M->E,
 *           the highest probability sequence would be
 *           artifactually short. (We could do the highest
 *           scoring sequence instead, to get around this problem,
 *           but the highest scoring sequence is prone to
 *           other artifacts -- any looping state N,C,J, or I
 *           with a positively scoring residue leads to
 *           an infinitely long "best scoring" sequence.)
 *
 * Args:     hmm     - the model
 *           ret_seq - RETURN: best sequence
 *           ret_L   - RETURN: length of sequence
 *           ret_tr  - RETURN: traceback of the model/seq alignment; or NULL.
 *
 * Returns:  void
 */
void
EmitBestSequence(struct plan7_s *hmm, char **ret_dsq, int *ret_L, struct p7trace_s **ret_tr)
{
  char              *seq;                  /* RETURN: best seq */
  struct p7trace_s  *tr;                   /* RETURN: traceback */
  float             *mmx, *imx, *dmx;      /* log P forward scores for M,D,I */
  char              *mtb, *itb, *dtb;      /* traceback ptrs for M,D,I */
  int  x;		        /* counter for symbols */
  int  k;			/* counter for nodes   */
  float sc;			/* tmp var for a log P */
  int  bestsym;
  int  rpos;			/* position in a sequence */
  int  tpos;			/* position in a trace */
  int  tlen;			/* length of the traceback */

  /* Initial allocations. We only need a 1D matrix and its shadow;
   * it's overkill to use the Plan7Matrix structures, so don't.
   */
  mmx = MallocOrDie(sizeof(float) * (hmm->M+1));
  imx = MallocOrDie(sizeof(float) * (hmm->M));
  dmx = MallocOrDie(sizeof(float) * (hmm->M));
  mtb = MallocOrDie(sizeof(char)  * (hmm->M+1));
  itb = MallocOrDie(sizeof(char)  * (hmm->M));
  dtb = MallocOrDie(sizeof(char)  * (hmm->M));

  /* Initialization. 
   * We can safely assume a max probability path of S->N->B->(M1 or D1),
   * so just init M1 and D1.
   */
  mmx[1] = log(hmm->xt[XTN][MOVE]) + log(1.F - hmm->tbd1);
  dmx[1] = 


  /* Main recursion, done as a push.
   * The model is used in probability form; no wing folding needed.
   */
  for (k = 1; k < hmm->M; k++)
    {
      /* Transits out of match state (init with these)
       */
      mmx[k+1] = mmx[k] + log(hmm->t[k][TMM]); mtb[k+1] = STM;
      dmx[k+1] = mmx[k] + log(hmm->t[k][TMD]); dtb[k+1] = STM;
      if (k < hmm->M-1) 
	imx[k]   = mmx[k] + log(hmm->t[k][TMI]); itb[k]   = STM;
      
      /* Transits out of delete state
       */
      if ((sc = dmx[k] + log(hmm->t[k][TDM])) > mmx[k+1]) 
	{ mmx[k+1] = sc; mtb[k+1] = STD; }
      if ((sc = dmx[k] + log(hmm->t[k][TDD])) > dmx[k+1])
	{ dmx[k+1] = sc; dtb[k+1] = STD; }

      /* Transits out of insert state (self-loops are never good)
       */
      if ((sc = imx[k] + log(hmm->t[k][TIM])) > mmx[k+1])
	{ mmx[k+1] = sc; mtb[k+1] = STI; }
      
      /* Best emissions
       */
      x = FMax(hmm->mat[k+1], Alphabet_size);
      mmx[k+1] += log(hmm->mat[k+1][x]);

      if (k < hmm->M-1) {
	x = FMax(hmm->ins[k+1], Alphabet_size);
	imx[k+1] += log(hmm->ins[k+1][x]);
      }
    }
}