Пример #1
0
void MeshManipulator::smoothSurface(const Ray * r)
{
	VertexAdjacency adj = m_topo->getAdjacency(m_intersect->m_componentIdx);
	
	Vector3F *p = &m_mesh->vertices()[m_intersect->m_componentIdx];
    Vector3F d = adj.center() - *p;
	*p += d * .7f;
	
	Plane pl(m_intersect->m_hitN, m_intersect->m_hitP);

    Vector3F hit;
    float t;
	if(!pl.rayIntersect(*r, hit, t, 1)) return;
	
	d = hit - *p;
	float minD = d.length();
	float curD;
	
	VertexAdjacency::VertexNeighbor *neighbor;
    for(neighbor = adj.firstNeighbor(); !adj.isLastNeighbor(); neighbor = adj.nextNeighbor()) {
        d = hit - *(neighbor->v->m_v);
        curD = d.length();
		if(curD < minD) {
			minD = curD;
			m_intersect->m_componentIdx = neighbor->v->getIndex();
		}
    }
}
Пример #2
0
Vector3F LinearSpline::getPositionAtTime(unsigned int length, unsigned int time)
{
	float splineLength = 0;
	std::vector<float> segmentLengths;
	for (size_t i = 1; i < mNodes.size(); ++i) {
		float segmentLength = (mNodes[i - 1]->position.value() - mNodes[i]->position.value()).length();
		splineLength += segmentLength;
		segmentLengths.push_back(splineLength);
	}
	float step = splineLength / length;
	float segment = time * step;
	size_t segmentIndex = 0;
	while (segment > segmentLengths[segmentIndex])
		segmentIndex++;

	float segmentRemains = segment - (segmentIndex > 0 ? segmentLengths[segmentIndex - 1] : 0);

	Vector3F segmentToUse = mNodes[segmentIndex + 1]->position.value() - mNodes[segmentIndex]->position.value();
	float segmentToUseLength = segmentToUse.length();

	float segmentPercentage = segmentRemains / segmentToUseLength;

	Vector3F segmentFraction = segmentToUse * segmentPercentage;

	return mNodes[segmentIndex]->position.value() + segmentFraction;
}
Пример #3
0
void BaseView::tumble(int dx, int dy, int portWidth)
{
	Vector3F side  = m_space.getSide();
	Vector3F up    = m_space.getUp();
	Vector3F front = m_space.getFront();
	Vector3F eye = m_space.getTranslation();	
	Vector3F toEye = eye - m_centerOfInterest;
	float dist = toEye.length();
	const float scaleing = dist * 2.f / (float)portWidth;
	eye -= side * (float)dx * scaleing;
	eye += up * (float)dy * scaleing;
	
	toEye = eye - m_centerOfInterest;
	toEye.normalize();
	
	eye = m_centerOfInterest + toEye * dist;
	m_space.setTranslation(eye);
	
	front = toEye;
	
	side = up.cross(front);
	side.y = 0.f;
	side.normalize();
	
	up = front.cross(side);
	up.normalize();
	
	m_space.setOrientations(side, up, front);
	
	m_invSpace = m_space;
	m_invSpace.inverse();
}
Пример #4
0
void GjkContactSolver::penetration(const PointSet & A, const PointSet & B, ClosestTestContext * result)
{
	resetSimplex(result->W);
	const Vector3F r = result->rayDirection;
	const Vector3F startP = Vector3F::Zero - result->rayDirection * 99.f;
	Vector3F hitP = startP;
	// from origin to startP
	Vector3F v = hitP;
	Vector3F w, p, pa, pb, localA, localB;
	float lamda = 0.f;
	float vdotw, vdotr;

	int k = 0;
	for(; k < 39; k++) {
		vdotr = v.dot(r);
	
		// SA-B(v)
		pa = A.supportPoint(v, result->transformA, localA, result->margin);
		pb = B.supportPoint(v.reversed(), result->transformB, localB, result->margin);
		p = pa - pb;// + v.normal() * MARGIN_DISTANCE;
		w = hitP - p;
		vdotw = v.dot(w); 
		
		if(vdotw > 0.f) {
			if(vdotr >= 0.f)
				break;
			lamda -= vdotw / vdotr;
			hitP = startP + r * lamda;
		}
				
		addToSimplex(result->W, p, localB);
	
		result->hasResult = 0;
		result->distance = 1e9;
		result->referencePoint = hitP;
	
		closestOnSimplex(result);
	
		v = hitP - result->closestPoint;
		
		interpolatePointB(result);
	
		if(v.length2() < TINY_VALUE) break;
		
		result->separateAxis = v;
	
		smallestSimplex(result);
	}
	
	result->distance = hitP.length();
	result->separateAxis.normalize();
}
Пример #5
0
void BaseView::zoom(int dz, int portWidth)
{
	Vector3F front = m_space.getFront();
	Vector3F eye = m_space.getTranslation();
	Vector3F toEye = eye - m_centerOfInterest;
	const float dist = toEye.length();
	
	const float fra = (float)dz/(float)portWidth * 7.f;
	
	eye += front * dist * -fra;
	if(fra > 0.f && dist < 10.f)
		m_centerOfInterest += front * dist * -fra * 0.1f;
	
	m_space.setTranslation(eye);
	m_invSpace = m_space;
	m_invSpace.inverse();
}
Пример #6
0
unsigned BaseMesh::closestVertex(unsigned idx, const Vector3F & px) const
{
	unsigned *trii = &_indices[idx * 3];
	float mag, minDist = 10e8;
	unsigned vert = 0;
	for(int i = 0; i < 3; i++) {
		Vector3F v = _vertices[*trii] - px;
		
		mag = v.length();
		
		if(mag < minDist) {
			minDist = mag;
			vert = *trii;
		}
		trii++;
	}
	return vert;
}
Пример #7
0
unsigned PatchMesh::closestVertex(unsigned idx, const Vector3F & px) const
{
	unsigned *qudi = &m_quadIndices[idx * 4];
	float mag, minDist = 10e8;
	unsigned vert = 0;
	for(int i = 0; i < 4; i++) {
		Vector3F v = getVertices()[*qudi] - px;
		
		mag = v.length();
		
		if(mag < minDist) {
			minDist = mag;
			vert = *qudi;
		}
		qudi++;
	}

	return vert;
}
Пример #8
0
void Vector3F::rotateAroundAxis(const Vector3F& axis, float theta)
{
	if(theta==0) return;
	Vector3F ori(x,y,z);
	float l = ori.length();
	ori.normalize();
	
	Vector3F up = axis.cross(ori);
	up.normalize();
	
	Vector3F side = ori - axis*(axis.dot(ori));
	
	up *=side.length();
	
	ori += side*(cos(theta) - 1);
	ori += up*sin(theta);
	
	ori.normalize();
	x = ori.x*l;
	y = ori.y*l;
	z = ori.z*l;
}
Пример #9
0
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
}