Vec2 Phy::calculateNormal(Circle* circle, Box* box, Vec2* close) {
	//Calculates normal from closest point of box to centre of ball
	//@ Vec2* close only for debug

	Vec2 closest = box->position;

	float xmax = (box->position.x + box->width / 2);
	float xmin = (box->position.x - box->width / 2);

	float ymax = (box->position.y + box->height / 2);
	float ymin = (box->position.y - box->height / 2);

	closest = Vec2(clamp(xmin, xmax, circle->position.x), clamp(ymin, ymax, circle->position.y));

	Vec2 normal;

	if (closest.equal(circle->position)) {
		//ball centre is inside box
		//std::cout << "AJAJAJ\n";
		Vec2 boxcentre_offset = vecDiff(closest, box->position);

		if (absf(boxcentre_offset.x) > absf(boxcentre_offset.y)) {
			//clamp to left or right
			if (boxcentre_offset.x > 0) {
				closest.x = xmax;
			}
			else {
				closest.x = xmin;
			}
		}
		else {
			//clamp to top or bottom
			if (boxcentre_offset.y > 0) {
				closest.y = ymax;
			}
			else {
				closest.y = ymin;
			}
		}

		normal = vecDiff(closest, circle->position);

	}
	else {
		//ball centre is outside box
		normal = vecDiff(circle->position, closest);
	}

	*close = closest;

	return normal;

}
Exemple #2
0
bool
CSphere::Intersect(const CRay& clRay, RealType t0, RealType t1, TTracingContext& tContext ) const
{
	/*
		Implement ray-sphere intersection.
		You must set the following member of the TTracingContext struct:
		t          - ray parameter of intersection point
		v3Normal   - Normal of surface at intersection point
		v3HitPoint - Coordinate of intersection point
		v2TexCoord - 2D-Texture coordinate of intersection point (use polar coordinates (method Cartesian2Polar), not needed currently)
		pclShader  - Material of sphere
	*/

    VectorType3 vecDiff(m_v3Center - clRay.GetOrigin());


    RealType t_ca = vecDiff | clRay.GetDir();

    if (t_ca < 0)
        return false;

    RealType d2 = (vecDiff | vecDiff) - t_ca * t_ca;
    RealType r2 = m_rRadius * m_rRadius ;

    if (d2 > r2) {
        return false;
    }

    RealType desc = sqrt(r2 - d2);

    RealType t = (t_ca - desc) ;

    if (t < 0.0) {
        t = (t_ca + desc);
    }

    if (t > tContext.t) {
        return false;
    }
    tContext.t = t;

    tContext.v3HitPoint = clRay.Evaluate(tContext.t);
    tContext.v3Normal = (tContext.v3HitPoint - m_v3Center).normalize();

    tContext.pclShader = GetShader();

    return true;

}
Exemple #3
0
void TrackRunner::lm() {
	printf("==== Least Square ====\n");
	auto vecPosesBack = vecPoses;
	std::vector<PoseState> vecDiff(vecMotionLinks.size());

	//std::function<void(MotionState&)> func1 = [&](MotionState& m) {
	//	int j = m.idxImg[0],
	//		i = m.idxImg[1];
	//	PoseState moved = vecPosesBack[j].move(m);
	//	vecDiff[i].pos += 2.0f*(vecPosesBack[i].pos - moved.pos);
	//	vecDiff[i].dir += 2.0f*(vecPosesBack[i].dir - moved.dir);
	//};

	//std::function<void(MotionState&)> func2 = [&](MotionState& m) {

	//};

	for (int i = 0; i < vecMotionLinks.size(); i++) {
		if (vecPosesBack[i].getInited() == false) break;
		if (CFG_bIsLogGlobal)
		std::cout << vecPosesBack[i] << std::endl;
	}

	for (int idxCur = 0; idxCur < vecMotionLinks.size(); idxCur++) {
		auto& mp = vecMotionLinks[idxCur];
		if (mp.size() == 0&&idxCur !=0) break;
		for (auto& pair : mp) {
			int idxPre = pair.first;
			MotionState& motion = pair.second;

			if (CFG_bIsLogGlobal)
			printf("Motion[%d-%d]\n", idxPre, idxCur);
			if (CFG_bIsLogGlobal)
			std::cout << motion << std::endl;

		}
	}
}
Exemple #4
0
extendTreeR_t ExtendTree_NHC_Sort_OnlyGS_TermCond_Heading(MatrixXd tree, VectorXd vecEndNode, worldLine_t world, setting_t P,
				double dKmax, double c4, double dMax, double dMaxBeta, sigma_t sigma, int iMaxIte, int iINDC)
{
	int iFlag, kk, iTemp, n, idx, flagRet;
	double p, r, theta, Sx, Sy;

	Vector3d vec3RandomPoint;
	VectorXd vecTempDiag, vecIdx, vecP1, vecP2, vecP3, vecNewNode;
	MatrixXd matTemp, matTempSq, matWP, newTree, matdKappa;
	VectorXd vecBeta;

	extendTreeR_t funcReturn;

	iFlag = 0;

	while (iFlag==0) {
		// select a biased random point
		p=Uniform01();

		if ( (iINDC==0 && p<0.1) || (iINDC==1 && p<0.05) ) {
			vec3RandomPoint << vecEndNode(0), vecEndNode(1), vecEndNode(2);
		} else {
			r = sigma.r*Uniform01();
			theta = sigma.theta0 + sigma.theta*quasi_normal_random();

			Sx = sigma.sp(0) + r*cos(theta);
			Sy = sigma.sp(1) + r*sin(theta);
			vec3RandomPoint << Sx, Sy, vecEndNode(2);
		}
		std::cout << "Random Point : " << vec3RandomPoint(0) << " " << vec3RandomPoint(1) << " " << vec3RandomPoint(2) << std::endl;

		// Find node that is closest to random point
		matTemp.resize(tree.rows(),3);

		for (iTemp=0; iTemp<tree.rows(); iTemp++)	{
			matTemp(iTemp,0) = tree(iTemp,0) - vec3RandomPoint(0);
			matTemp(iTemp,1) = tree(iTemp,1) - vec3RandomPoint(1);
			matTemp(iTemp,2) = tree(iTemp,2) - vec3RandomPoint(2);
		}
		matTempSq = matTemp*matTemp.transpose();

		vecTempDiag.resize(matTemp.rows());
		for (iTemp=0; iTemp<matTemp.rows(); iTemp++)
			vecTempDiag(iTemp) = matTempSq(iTemp, iTemp);

		SortVec(vecTempDiag, vecIdx); // vecTempDiag : sorted vector, vecIdx : index of vecTempDiag

		// Modification 3
		if ( vecIdx.rows() > iMaxIte )
			n = iMaxIte;
		else
			n = vecIdx.rows();

		/// Nonholonomic Length Decision
		kk=-1;

		// Modification 4
		if (tree.rows() == 2) {
			vecP1.resize(3); vecP2.resize(3); vecP3.resize(3);
			vecP1 << tree(0,0), tree(0,1), tree(0,2);
			vecP2 << tree(1,0), tree(1,1), tree(1,2);
			vecP3 = vec3RandomPoint;

			matWP.resize(3,3);
			matWP.row(0) = vecP1.segment(0,3).transpose();
			matWP.row(1) = vecP2.segment(0,3).transpose();
			matWP.row(2) = vecP3.segment(0,3).transpose();
			Kappa_Max_Calculation(matWP, P, matdKappa, vecBeta);

			if ( (vecBeta.array().abs()<= dMaxBeta).all() )
				// Method 2 : use the maximum length margin - when there is straight line, there is more margin1
				P.L = 2*dMax;
			else if ( (vecBeta.array().abs() > dMaxBeta).all() ) {
				funcReturn.flag = 0;
				funcReturn.newTree = tree;
				funcReturn.INDC = iINDC;
				funcReturn.sigma = sigma;
				funcReturn.maxIte = iMaxIte;

				return funcReturn;
			}
		} else if (tree.rows() > 2)	{
			for (iTemp=0; iTemp<n; iTemp++)	{
				kk = iTemp;

				if ( tree(vecIdx(iTemp), tree.cols()-1) == 0 )	{
					funcReturn.flag = 0;
					funcReturn.newTree = tree;
					funcReturn.INDC = iINDC;
					funcReturn.sigma = sigma;
					funcReturn.maxIte = iMaxIte;

					return funcReturn;
				}

				vecP2.resize(tree.cols());
				vecP2 = tree.row(vecIdx(iTemp)).transpose();
				vecP1.resize(tree.cols());
				vecP1 = tree.row( vecP2(vecP2.rows()-1)-1 ).transpose();
				vecP3.resize(vec3RandomPoint.rows());
				vecP3 = vec3RandomPoint;

				matWP.resize(3,3);

				matWP.row(0) = vecP1.segment(0,3).transpose();
				matWP.row(1) = vecP2.segment(0,3).transpose();
				matWP.row(2) = vecP3.segment(0,3).transpose();

				Kappa_Max_Calculation(matWP, P, matdKappa, vecBeta);

				if ( (vecBeta.array().abs()<= dMaxBeta).all() )
				{
					// Method 2 : use the maximum length margin - when there is straight line, there is more margin1
					P.L = 2*dMax;
					break;
				} else if ( (vecBeta.array().abs() > dMaxBeta).all() && (iTemp == (n-1) ) ) 	{
					funcReturn.flag = 0;
					funcReturn.newTree = tree;
					funcReturn.INDC = iINDC;
					funcReturn.sigma = sigma;
					funcReturn.maxIte = iMaxIte;

					return funcReturn;
				}
			}
		} //if (tree.rows() == 2)

		if ( kk == -1)
			idx = vecIdx(0);
		else
			idx = vecIdx(kk);

		double cost = tree(idx,4) + P.L;
		Vector3d vecNewPoint = vec3RandomPoint - tree.block(idx,0,1,3).transpose();
		vecNewPoint = tree.block(idx,0,1,3).transpose()+vecNewPoint/vecNewPoint.norm()*P.L;

		if ( kk == -1)
		{
			vecNewNode.resize(vecNewPoint.size()+5);
			vecNewNode << vecNewPoint, 0, cost, P.L, 0, idx+1;
		}
		else
		{
			vecNewNode.resize(vecNewPoint.size()+vecBeta.size()+4);
			vecNewNode << vecNewPoint, 0, cost, P.L, vecBeta.array().abs(), idx+1;
		}

		// check to see if obstacle or random point is reached
		if ( Collision(vecNewNode, tree.row(idx), world, P.SZR, P.SZH, 0) == 0 )
		{
			newTree.resize( tree.rows()+1,tree.cols() );
			newTree.block(0, 0, tree.rows(), tree.cols()) = tree;
			newTree.row(tree.rows()) = vecNewNode.transpose();

			iFlag = 1;
		}
	} // while (iFlag==0)

	// check to see if new node connects directly to end node
	VectorXd vecDiff(3), vecSeg1(3), vecSeg2(3);

	vecSeg1 = vecNewNode.segment(0,3);
	vecSeg2 = vecEndNode.segment(0,3);
	vecDiff = vecSeg1 - vecSeg2;
	double dTerm = vecDiff.norm();

	double psi;
	// 7*dMax
	if ( (dTerm <= 7*dMax) && (iINDC==0) )
	{
		iINDC = 1;

		psi = atan2( vecEndNode(1)-vecNewNode(1), vecEndNode(0)-vecNewNode(0) );
		psi = psi - 2*M_PI*(psi>M_PI);

		sigma.r = dTerm;
		sigma.theta0 = psi;
		sigma.theta = 1.0*M_PI;
		sigma.sp = vecNewNode.segment(0,3);

		iMaxIte = 5;
	}

	if ( iINDC==1 )
	{
		// Method 2
		int end;

		vecP2.resize(vecNewNode.rows());
		vecP2 = vecNewNode;

		end = vecP2.rows();

		if ( vecP2(end-1)==0 )
		{
			vecP1.resize(tree.rows());
			vecP1 = tree.row(0).transpose();
		}
		else
		{
			vecP1.resize(tree.rows());
			vecP1 = tree.row(vecP2(end-1)-1).transpose();
		}

		vecP3.resize(3);
		vecP3 = vecEndNode.segment(0,3);

		matWP.resize(3,3);
		matWP.row(0) = vecP1.segment(0,3);
		matWP.row(1) = vecP2.segment(0,3);
		matWP.row(2) = vecP3.segment(0,3);

		double d_rm;

		Kappa_Max_Calculation(matWP, P, matdKappa, vecBeta);

		d_rm = ( c4*sin(vecP2(end-2)) ) / ( dKmax*cos(vecP2(end-2))*cos(vecP2(end-2)) );

		if ( (2*dMax-d_rm >= 1.0*matdKappa(0,0)) &&
			(Collision(vecNewNode, vecEndNode, world, P.SZR, P.SZH, 0) == 0) &&
			(matdKappa(0,0) <= dTerm) )
		{
			flagRet = 1;
			newTree(newTree.rows()-1,3) = 1;
		}
		else
			flagRet = 0;
	} else
		flagRet = 0;
	// if ( iINDC==1 )

	funcReturn.flag = flagRet;
	funcReturn.newTree = newTree;
	funcReturn.INDC = iINDC;
	funcReturn.sigma = sigma;
	funcReturn.maxIte = iMaxIte;

	return funcReturn;
}