C++ (Cpp) PointSet::angularMotionDiscの例

コード例 #1

ファイル: GjkContactSolver.cpp プロジェクト: ahmidou/aphid

void GjkContactSolver::timeOfImpact(const PointSet & A, const PointSet & B, ContinuousCollisionContext * result)
{
    result->hasContact = 0;
    result->penetrateDepth = 0.f;
    result->TOI = 0.f;
	
	// std::cout<<"\nb test p"<<result->positionB;
	// std::cout<<"\nb test v"<<result->linearVelocityB * 60.f;
	// std::cout<<"\nb test w"<<result->angularVelocityB * 60.f;
    
    const Vector3F relativeLinearVelocity = result->linearVelocityB - result->linearVelocityA;
    
    // std::cout<<" velocityA "<<result->linearVelocityA.str();
	// std::cout<<" velocityB "<<result->linearVelocityB.str();
    // std::cout<<" relativeLinearVelocity "<<relativeLinearVelocity.str();
		
    const float angularMotionSize = result->angularVelocityA.length() * A.angularMotionDisc()
                                    + result->angularVelocityB.length() * B.angularMotionDisc();
    // no relative motion
    if(relativeLinearVelocity.length() + angularMotionSize < TINY_VALUE)
		return;
		
#ifdef DBG_DRAW		
	Vector3F lineB = result->positionB;
	Vector3F lineE = lineB + relativeLinearVelocity;
	glColor3f(0.f, .1f, .6f);
	m_dbgDrawer->arrow(lineB, lineE);
	lineB = result->positionA;
	lineE = lineB - relativeLinearVelocity;
	glColor3f(0.f, .1f, .6f);
	m_dbgDrawer->arrow(lineB, lineE);
#endif
		
    ClosestTestContext separateIo;
	separateIo.needContributes = 1;
	separateIo.margin = 0.05f;
    
    Vector3F separateN;
    
    float distance, closeInSpeed;
	float lastDistance = 0.f;
	float dDistanceaLamda;
    
    const Vector3F position0A = result->positionA;
    const Vector3F position0B = result->positionB;
    const Quaternion orientation0A = result->orientationA;
    const Quaternion orientation0B = result->orientationB;
    
    float lamda = 0.f;
	float limitDeltaLamda, deltaLamda = 1.f;
	float lastLamda = 0.f;
	
	int k = 0;
    for(; k < 32; k++) {
        
        separateIo.transformA.setTranslation(position0A.progress(result->linearVelocityA, lamda));
		Quaternion ra = orientation0A.progress(result->angularVelocityA, lamda);
		ra.normalize();
        separateIo.transformA.setRotation(ra);
        separateIo.transformB.setTranslation(position0B.progress(result->linearVelocityB, lamda));
		Quaternion rb = orientation0B.progress(result->angularVelocityB, lamda);
		rb.normalize();
		separateIo.transformB.setRotation(rb);
		// std::cout<<"\nk "<<k;
		// std::cout<<"\nb at p"<<separateIo.transformB.getTranslation();
		// std::cout<<"\nmat"<<separateIo.transformB.str();
        
        separateIo.referencePoint.setZero();
		separateIo.distance = 1e9;
		separateDistance(A, B, &separateIo);
        
        if(separateIo.hasResult) {
            if(k<1) {	
				std::cout<<"     contact at t0 try zero margin\n";
                separateIo.margin = 0.f;
				separateIo.distance = 1e9;
				separateDistance(A, B, &separateIo);
				if(separateIo.hasResult) {
					std::cout<<"     penetrated\n";
					result->hasContact = 0;
					return;
				}
				
				result->contactPointB = separateIo.contactPointB;
				distance = separateIo.separateAxis.length();
				result->penetrateDepth = 0.1 - distance;
				separateN = separateIo.separateAxis / distance;
#ifdef DBG_GJK_DRAW		
		lineB = separateIo.transformB.transform(separateIo.contactPointB);
		lineE = lineB + separateN;
		glColor3f(1.f, 0.f, 0.f);
		m_dbgDrawer->arrow(lineB, lineE);
#endif
				break;
            } else {
				// std::cout<<" contact at "<<lamda;;
				lamda = lastLamda;
				break;
			} 
		}
		
		result->contactPointB = separateIo.contactPointB;
		
		distance = separateIo.separateAxis.length();
				
        if(distance < .001f) {
			// std::cout<<" "<<k<<" close enough at "<<lamda<<"\n";
			if(k<1) {
				separateIo.margin = 0.f;
				separateIo.distance = 1e9;
				separateDistance(A, B, &separateIo);
				result->contactPointB = separateIo.contactPointB;
				distance = separateIo.separateAxis.length();
				separateN = separateIo.separateAxis / distance;
			}
			break;
		}
		
		separateN = separateIo.separateAxis / distance;
		
#ifdef DBG_GJK_DRAW		
		lineB = separateIo.transformB.transform(separateIo.contactPointB);
		lineE = lineB + separateN;
		glColor3f(1.f, 0.f, 0.f);
		m_dbgDrawer->arrow(lineB, lineE);
#endif

		dDistanceaLamda = (distance - lastDistance) / deltaLamda;
		lastDistance = distance;
		
		// std::cout<<" sep ax "<<separateIo.separateAxis.str();
		// std::cout<<" dist "<<distance;
		// std::cout<<" sep n "<<separateN.str();
				
		closeInSpeed = relativeLinearVelocity.dot(separateN);
		// std::cout<<" closeInSpeed "<<closeInSpeed;

        if(closeInSpeed + angularMotionSize < 0.f) {
			// std::cout<<"go apart at time "<<lamda<<"\n";
			return;
		}
		
		deltaLamda = distance / (closeInSpeed + angularMotionSize);
		if(dDistanceaLamda < 0.f) {
			limitDeltaLamda = -.59f * lastDistance / dDistanceaLamda;
			if(deltaLamda > limitDeltaLamda) {
				deltaLamda = limitDeltaLamda;
				// std::cout<<" limit delta lamda "<<deltaLamda<<"\n";
			}
		}
		lastLamda = lamda;
		lamda += deltaLamda;

        if(lamda < 0.f) {
			// std::cout<<"lamda < 0\n";
			return;
		}
        if(lamda > 1.f) {
			// std::cout<<"lamda > 1\n";
			return;
		}
    }
	
    result->hasContact = 1;
	result->TOI = lamda;
	result->contactNormal = separateN.reversed();
	// std::cout<<" v.n "<<result->contactNormal.dot(relativeLinearVelocity); 
	// std::cout<<" TOI "<<result->TOI;
	// result->contactNormal.verbose(" N");
	// relativeLinearVelocity.verbose(" Vrel");
#ifdef DBG_DRAW		
	lineB = separateIo.transformB.transform(separateIo.contactPointB);
	lineE = lineB + result->contactNormal;
	glColor3f(.2f, 1.f, .1f);
	m_dbgDrawer->arrow(lineB, lineE);
#endif
}