C++ CCVector3类代码示例

const CCVector3 CCCameraBase::getDirection()
{
    CCVector3 direction = lookAt;
    direction.sub( rotatedPosition );
    CCVector3Normalize( direction );
    return direction;
}

void ccClipBox::update()
{
	if (m_entityContainer.getChildrenNumber() == 0)
	{
		return;
	}

	//remove any existing clipping plane
	{
		for (unsigned ci = 0; ci < m_entityContainer.getChildrenNumber(); ++ci)
		{
			m_entityContainer.getChild(ci)->removeAllClipPlanes();
		}
	}
	
	//now add the 6 box clipping planes
	ccBBox extents;
	ccGLMatrix transformation;
	get(extents, transformation);

	CCVector3 C = transformation * extents.getCenter();
	CCVector3 halfDim = extents.getDiagVec() / 2;

	//for each dimension
	for (unsigned d = 0; d < 3; ++d)
	{
		CCVector3 N = transformation.getColumnAsVec3D(d);
		//positive side
		{
			ccClipPlane posPlane;
			posPlane.equation.x = N.x;
			posPlane.equation.y = N.y;
			posPlane.equation.z = N.z;

			//compute the 'constant' coefficient knowing that P belongs to the plane if (P - (C - half_dim * N)).N = 0
			posPlane.equation.w = -static_cast<double>(C.dot(N)) + halfDim.u[d];
			for (unsigned ci = 0; ci < m_entityContainer.getChildrenNumber(); ++ci)
			{
				m_entityContainer.getChild(ci)->addClipPlanes(posPlane);
			}
		}

		//negative side
		{
			ccClipPlane negPlane;
			negPlane.equation.x = -N.x;
			negPlane.equation.y = -N.y;
			negPlane.equation.z = -N.z;

			//compute the 'constant' coefficient knowing that P belongs to the plane if (P - (C + half_dim * N)).N = 0
			//negPlane.equation.w = -(static_cast<double>(C.dot(N)) + halfDim.u[d]);
			negPlane.equation.w = static_cast<double>(C.dot(N)) + halfDim.u[d];
			for (unsigned ci = 0; ci < m_entityContainer.getChildrenNumber(); ++ci)
			{
				m_entityContainer.getChild(ci)->addClipPlanes(negPlane);
			}
		}
	}
}

bool HornRegistrationTools::FindAbsoluteOrientation(GenericCloud* lCloud,
													GenericCloud* rCloud,
													ScaledTransformation& trans,
													bool fixedScale/*=false*/)
{
    //resulting transformation (R is invalid on initialization and T is (0,0,0))
    trans.R.invalidate();
    trans.T = CCVector3(0,0,0);
	trans.s = (PointCoordinateType)1.0;

	assert(rCloud && lCloud);
	if (!rCloud || !lCloud || rCloud->size() != lCloud->size() || rCloud->size()<3)
		return false;
	unsigned count = rCloud->size();
	assert(count>2);

	//determine best scale?
	if (!fixedScale)
	{
		CCVector3 Gr = GeometricalAnalysisTools::computeGravityCenter(rCloud);
		CCVector3 Gl = GeometricalAnalysisTools::computeGravityCenter(lCloud);

		//we determine scale with the symmetrical form as proposed by Horn
		double lNorm2Sum = 0.0;
		{		
			lCloud->placeIteratorAtBegining();
			for (unsigned i=0;i<count;i++)
			{
				CCVector3 Pi = *lCloud->getNextPoint()-Gl;
				lNorm2Sum += Pi.dot(Pi);
			}
		}

		if (lNorm2Sum >= ZERO_TOLERANCE)
		{
			double rNorm2Sum = 0.0;
			{
				rCloud->placeIteratorAtBegining();
				for (unsigned i=0;i<count;i++)
				{
					CCVector3 Pi = *rCloud->getNextPoint()-Gr;
					rNorm2Sum += Pi.dot(Pi);
				}
			}

			//resulting scale
			trans.s = (PointCoordinateType)sqrt(rNorm2Sum/lNorm2Sum);
		}
		//else
		//{
		//	//shouldn't happen!
		//}
	}

	return RegistrationProcedure(lCloud,rCloud,trans,0,0,trans.s);
}

//return angle between two vectors (in degrees)
//warning: vectors will be normalized by default
double GetAngle_deg(CCVector3& AB, CCVector3& AC)
{
	AB.normalize();
	AC.normalize();
	double dotprod = AB.dot(AC);
	if (dotprod<=-1.0)
		return 180.0;
	else if (dotprod>1.0)
		return 0.0;
	return 180.0*acos(dotprod)/M_PI;
}

void CCMiscTools::ComputeBaseVectors(const CCVector3 &N, CCVector3& X, CCVector3& Y)
{
	CCVector3 Nunit = N;
	Nunit.normalize();

	//we create a first vector orthogonal to the input one
	X = Nunit.orthogonal(); //X is also normalized

	//we deduce the orthogonal vector to the input one and X
	Y = N.cross(X);
	//Y.normalize(); //should already be normalized!
}

//return angle between two vectors (in degrees)
//warning: vectors will be normalized by default
static double GetAngle_deg(CCVector3 AB, CCVector3 AC)
{
	AB.normalize();
	AC.normalize();
	double dotprod = AB.dot(AC);
	//clamp value (just in case)
	if (dotprod <= -1.0)
		dotprod = -1.0;
	else if (dotprod > 1.0)
		dotprod = 1.0;
	return acos(dotprod) * CC_RAD_TO_DEG;
}

ccGLMatrix ccGLMatrix::zRotation() const
{
	//we can use the standard Euler angles convention here
	float phi,theta,psi;
	CCVector3 T;
	getParameters(phi,theta,psi,T);
	assert(T.norm2()==0);

	ccGLMatrix newRotMat;
	newRotMat.initFromParameters(phi,0,0,T);

	return newRotMat;
}

void ccClipBox::update()
{
	if (!m_associatedEntity)
	{
		return;
	}

#ifdef USE_OPENGL
	m_associatedEntity->removeAllClipPlanes();
	
	//now add the 6 box clipping planes
	ccBBox extents;
	ccGLMatrix transformation;
	get(extents, transformation);

	CCVector3 C = transformation * extents.getCenter();
	CCVector3 halfDim = extents.getDiagVec() / 2;

	//for each dimension
	for (unsigned d = 0; d < 3; ++d)
	{
		CCVector3 N = transformation.getColumnAsVec3D(d);
		//positive side
		{
			ccClipPlane posPlane;
			posPlane.equation.x = N.x;
			posPlane.equation.y = N.y;
			posPlane.equation.z = N.z;

			//compute the 'constant' coefficient knowing that P belongs to the plane if (P - (C - half_dim * N)).N = 0
			posPlane.equation.w = -static_cast<double>(C.dot(N)) + halfDim.u[d];
			m_associatedEntity->addClipPlanes(posPlane);
		}

		//negative side
		{
			ccClipPlane negPlane;
			negPlane.equation.x = -N.x;
			negPlane.equation.y = -N.y;
			negPlane.equation.z = -N.z;

			//compute the 'constant' coefficient knowing that P belongs to the plane if (P - (C + half_dim * N)).N = 0
			//negPlane.equation.w = -(static_cast<double>(C.dot(N)) + halfDim.u[d]);
			negPlane.equation.w = static_cast<double>(C.dot(N)) + halfDim.u[d];
			m_associatedEntity->addClipPlanes(negPlane);
		}
	}
#else
	flagPointsInside();
#endif
}

void ccGBLSensor::projectPoint(	const CCVector3& sourcePoint,
								CCVector2& destPoint,
								PointCoordinateType &depth,
								double posIndex/*=0*/) const
{
	//project point in sensor world
	CCVector3 P = sourcePoint;

	//sensor to world global transformation = sensor position * rigid transformation
	ccIndexedTransformation sensorPos; //identity by default
	if (m_posBuffer)
		m_posBuffer->getInterpolatedTransformation(posIndex,sensorPos);
	sensorPos *= m_rigidTransformation;

	//apply (inverse) global transformation (i.e world to sensor)
	sensorPos.inverse().apply(P);

	//convert to 2D sensor field of view + compute its distance
	switch (m_rotationOrder)
	{
	case YAW_THEN_PITCH:
	{
		//yaw = angle around Z, starting from 0 in the '+X' direction
		destPoint.x = atan2(P.y,P.x);
		//pitch = angle around the lateral axis, between -pi (-Z) to pi (+Z) by default
		destPoint.y = atan2(P.z,sqrt(P.x*P.x + P.y*P.y));
		break;
	}
	case PITCH_THEN_YAW:
	{
		//FIXME
		//yaw = angle around Z, starting from 0 in the '+X' direction
		destPoint.x = -atan2(sqrt(P.y*P.y + P.z*P.z),P.x);
		//pitch = angle around the lateral axis, between -pi (-Z) to pi (+Z) by default
		destPoint.y = -atan2(P.y,P.z);
		break;
	}
	default:
		assert(false);
	}
	
	//if the yaw angles are shifted
	if (m_yawAnglesAreShifted && destPoint.x < 0)
		destPoint.x += static_cast<PointCoordinateType>(2.0*M_PI);
	//if the pitch angles are shifted
	if (m_pitchAnglesAreShifted && destPoint.y < 0)
		destPoint.y += static_cast<PointCoordinateType>(2.0*M_PI);

	depth = P.norm();
}

float ccFastMarchingForNormsDirection::computePropagationConfidence(DirectionCell* originCell, DirectionCell* destCell) const
{
	//1) it depends on the angle between the current cell's orientation
	//	and its neighbor's orientation (symmetric)
	//2) it depends on whether the neighbor's relative position is
	//	compatible with the current cell orientation (symmetric)
	CCVector3 AB = destCell->C - originCell->C;
	AB.normalize();

	float psOri = fabs(static_cast<float>(AB.dot(originCell->N))); //ideal: 90 degrees
	float psDest = fabs(static_cast<float>(AB.dot(destCell->N))); //ideal: 90 degrees
	float oriConfidence = (psOri + psDest)/2; //between 0 and 1 (ideal: 0)
	
	return 1.0f - oriConfidence;
}

void CCTile3D::setPositionXYZ(const float x, const float y, const float z)
{
    if( position.x != x || position.y != y || position.z != z )
    {
        CCVector3 distance = position;
        super::setPositionXYZ( x, y, z );
        distance.sub( position );

        for( int i=0; i<attachments.length; ++i )
        {
            CCSceneObject *attachment = attachments.list[i];
            attachment->translate( -distance.x, -distance.y, -distance.z );
        }

        CCOctreeRefreshObject( this );
    }
}

bool CharacterUpdaterPlayer::reportAttack(CCObject *attackedBy, const float force, const float damage, const float x, const float y, const float z)
{
    if( health > 0.0f )
    {
        health -= damage;
    }

    CCVector3 attackVelocity = CCVector3( x, y, z );
    attackVelocity.unitize();
    attackVelocity.mul( force * 0.1f );
    player->incrementAdditionalVelocity( attackVelocity.x, attackVelocity.y, attackVelocity.z );
    findNewAnchor = true;

    player->getGame()->registerAttack( attackedBy, player, damage );

    return false;
}

static bool lineCheckGetIntersection(const float dist1, const float dist2, const CCVector3 &point1, const CCVector3 &point2, CCVector3 &hitLocation) 
{
	if( ( dist1 * dist2 ) >= 0.0f )
	{
		return false;
	}
	
	if( dist1 == dist2 )
	{ 
		return false;
	}
	
    // point1 + ( point2 - point1 ) * ( -dst1 / ( dst2 - dst1 ) );
    hitLocation = point2;
    hitLocation.sub( point1 );
    hitLocation.mul( -dist1 / ( dist2 - dist1 ) );
    hitLocation.add( point1 );
	
	return true;
}

float FastMarchingForFacetExtraction::computeTCoefApprox(CCLib::FastMarching::Cell* originCell, CCLib::FastMarching::Cell* destCell) const
{
	PlanarCell* oCell = static_cast<PlanarCell*>(originCell);
	PlanarCell* dCell = static_cast<PlanarCell*>(destCell);

	//compute the 'confidence' relatively to the neighbor cell
	//1) it depends on the angle between the current cell's orientation
	//	and its neighbor's orientation (symmetric)
	//2) it depends on whether the neighbor's relative position is
	//	compatible with the current cell orientation (symmetric)
	float orientationConfidence = 0;
	{
		CCVector3 AB = dCell->C - oCell->C;
		AB.normalize();

		float psOri = fabs(static_cast<float>(AB.dot(oCell->N))); //ideal: 90 degrees
		float psDest = fabs(static_cast<float>(AB.dot(dCell->N))); //ideal: 90 degrees
		orientationConfidence = (psOri + psDest)/2; //between 0 and 1 (ideal: 0)
	}

	//add reprojection error into balance
	if (m_useRetroProjectionError && m_octree && oCell->N.norm2() != 0)
	{
		PointCoordinateType theLSQPlaneEquation[4];
		theLSQPlaneEquation[0] = oCell->N.x;
		theLSQPlaneEquation[1] = oCell->N.y;
		theLSQPlaneEquation[2] = oCell->N.z;
		theLSQPlaneEquation[3] = oCell->C.dot(oCell->N);

		CCLib::ReferenceCloud Yk(m_octree->associatedCloud());
		if (m_octree->getPointsInCell(oCell->cellCode,m_gridLevel,&Yk,true))
		{
			ScalarType reprojError = CCLib::DistanceComputationTools::ComputeCloud2PlaneDistance(&Yk,theLSQPlaneEquation,m_errorMeasure);
			if (reprojError >= 0)
				return (1.0f-orientationConfidence) * static_cast<float>(reprojError);
		}
	}

	return (1.0f-orientationConfidence) /** oCell->planarError*/;
}

//helper: computes a facet horizontal and vertical extensions
void ComputeFacetExtensions(CCVector3& N, ccPolyline* facetContour, double& horizExt, double& vertExt)
{
	//horizontal and vertical extensions
	horizExt = vertExt = 0;
	
	CCLib::GenericIndexedCloudPersist* vertCloud = facetContour->getAssociatedCloud();
	if (vertCloud)
	{
		//oriRotMat.applyRotation(N); //DGM: oriRotMat is only for display!
		//we assume that at this point the "up" direction is always (0,0,1)
		CCVector3 Xf(1,0,0), Yf(0,1,0);
		//we get the horizontal vector on the plane
		CCVector3 D = CCVector3(0,0,1).cross(N);
		if (D.norm2() > ZERO_TOLERANCE) //otherwise the facet is horizontal!
		{
			Yf = D;
			Yf.normalize();
			Xf = N.cross(Yf);
		}

		const CCVector3* G = CCLib::Neighbourhood(vertCloud).getGravityCenter();

		ccBBox box;
		for (unsigned i=0; i<vertCloud->size(); ++i)
		{
			const CCVector3 P = *(vertCloud->getPoint(i)) - *G;
			CCVector3 p( P.dot(Xf), P.dot(Yf), 0 );
			box.add(p);
		}

		vertExt = box.getDiagVec().x;
		horizExt = box.getDiagVec().y;
	}
}