C++ PxVec3::normalize方法代码示例

bool Actor::patternFracture(const PxVec3& hitLocation, const PxVec3& dirIn, float scale, float vel, float radiusIn)
{
	int compoundNr = -1;
	int convexNr = -1;
	PxVec3 normal;
	float dist;
	bool ret = false;
	PxVec3 dir = dirIn;

	mScene->getScene()->lockWrite();

	bool hit = false;
	if (dir.magnitudeSquared() < 0.5f)
	{
		dir = PxVec3(1.f,0.f,0.f);
		hit = base::Actor::rayCast(hitLocation-dir,dir,dist,compoundNr,convexNr,normal);
		if(!hit)
		{
			dir = PxVec3(0.f,1.f,0.f);
			hit = base::Actor::rayCast(hitLocation-dir,dir,dist,compoundNr,convexNr,normal);
			if(!hit)
			{
				dir = PxVec3(0.f,0.f,1.f);
				hit = base::Actor::rayCast(hitLocation-dir,dir,dist,compoundNr,convexNr,normal);
			}
		}
	}
	else
	{
		hit = base::Actor::rayCast(hitLocation-dir,dir,dist,compoundNr,convexNr,normal);
	}
	if (hit)
	{
		float radius = mMinRadius + mRadiusMultiplier*radiusIn;
		float impulseMagn = scale*vel*mImpulseScale;

		if (mSheetFracture)
		{
			normal = ((Compound*)mCompounds[(uint32_t)compoundNr])->mNormal;
		}
		PxVec3 a(0.f,0.f,1.f);
		normal.normalize();
		a -= a.dot(normal)*normal;
		if( a.magnitudeSquared() < 0.1f )
		{
			a = PxVec3(0.f,1.f,0.f);
			a -= a.dot(normal)*normal;
		}
		a.normalize();
		PxVec3 b(normal.cross(a));
		PxMat33 trans(a,b,normal);
		ret = base::Actor::patternFracture(hitLocation,dir,compoundNr,trans,radius,impulseMagn,impulseMagn);
	}

	mScene->getScene()->unlockWrite();

	mRenderResourcesDirty = true;

	return ret;
}

void AddRadialImpulseToPxRigidBody_AssumesLocked(PxRigidBody& PRigidBody, const FVector& Origin, float Radius, float Strength, uint8 Falloff, bool bVelChange)
{
#if WITH_PHYSX
	if (!(PRigidBody.getRigidBodyFlags() & PxRigidBodyFlag::eKINEMATIC))
	{
		float Mass = PRigidBody.getMass();
		PxTransform PCOMTransform = PRigidBody.getGlobalPose().transform(PRigidBody.getCMassLocalPose());
		PxVec3 PCOMPos = PCOMTransform.p; // center of mass in world space
		PxVec3 POrigin = U2PVector(Origin); // origin of radial impulse, in world space
		PxVec3 PDelta = PCOMPos - POrigin; // vector from origin to COM

		float Mag = PDelta.magnitude(); // Distance from COM to origin, in Unreal scale : @todo: do we still need conversion scale?

		// If COM is outside radius, do nothing.
		if (Mag > Radius)
		{
			return;
		}

		PDelta.normalize();

		// Scale by U2PScale here, because units are velocity * mass. 
		float ImpulseMag = Strength;
		if (Falloff == RIF_Linear)
		{
			ImpulseMag *= (1.0f - (Mag / Radius));
		}

		PxVec3 PImpulse = PDelta * ImpulseMag;

		PxForceMode::Enum Mode = bVelChange ? PxForceMode::eVELOCITY_CHANGE : PxForceMode::eIMPULSE;
		PRigidBody.addForce(PImpulse, Mode);
	}
#endif // WITH_PHYSX
}

void BasicRandom::unitRandomPt(PxVec3& v)
{
	v.x = randomFloat();
	v.y = randomFloat();
	v.z = randomFloat();
	v.normalize();
}

void AddRadialForceToPxRigidBody_AssumesLocked(PxRigidBody& PRigidBody, const FVector& Origin, float Radius, float Strength, uint8 Falloff, bool bAccelChange)
{
#if WITH_PHYSX
	if (!(PRigidBody.getRigidBodyFlags() & PxRigidBodyFlag::eKINEMATIC))
	{
		float Mass = PRigidBody.getMass();
		PxTransform PCOMTransform = PRigidBody.getGlobalPose().transform(PRigidBody.getCMassLocalPose());
		PxVec3 PCOMPos = PCOMTransform.p; // center of mass in world space
		PxVec3 POrigin = U2PVector(Origin); // origin of radial impulse, in world space
		PxVec3 PDelta = PCOMPos - POrigin; // vector from

		float Mag = PDelta.magnitude(); // Distance from COM to origin, in Unreal scale : @todo: do we still need conversion scale?

		// If COM is outside radius, do nothing.
		if (Mag > Radius)
		{
			return;
		}

		PDelta.normalize();

		// If using linear falloff, scale with distance.
		float ForceMag = Strength;
		if (Falloff == RIF_Linear)
		{
			ForceMag *= (1.0f - (Mag / Radius));
		}

		// Apply force
		PxVec3 PImpulse = PDelta * ForceMag;
		PRigidBody.addForce(PImpulse, bAccelChange ? PxForceMode::eACCELERATION : PxForceMode::eFORCE);
	}
#endif // WITH_PHYSX
}

void Enemy::CheckFutureCollision(int attempts)
{
	attempts++;
	if(attempts > 10) return;
	PxVec3 pos = actor->getGlobalPose().p;
	PxVec3 dirMove = PxVec3(moveDir.x,0,moveDir.z);
	while(dirMove.isZero())
	{
		float xspeed = rand()% (int)ceil(movementSpeed); //getal van 0 tot 10
		float xfSpeed = xspeed - (int)ceil(movementSpeed)/2; // getal van -5 tot 5
		float zspeed = rand()%(int)ceil(movementSpeed); //getal van 0 tot 10
		float zfSpeed = zspeed -(int)ceil( movementSpeed)/2; // getal van -5 tot 5
		dirMove = PxVec3(xfSpeed, 0, zfSpeed);
	}
	dirMove.normalize();
	int numHits;
	PxRaycastHit* hit = physics->RaycastMultiple(pos-PxVec3(0,1.5f,0),dirMove,3.5f,&numHits,PxQueryFlag::eSTATIC | PxQueryFlag::eDYNAMIC);
	for(unsigned int i= 0; i < numHits; i++)
	{
		if(hit[i].actor != actor && hit[i].actor != NULL)
		{
			currentMoveTime = 0;	
			float xspeed = rand()% (int)ceil(movementSpeed);		//number from 0 to movementSpeed
			float xfSpeed = xspeed - (int)ceil(movementSpeed)/2;	// number from -movementSpeed/2 to movementSpeed/2
			float zspeed = rand()%(int)ceil(movementSpeed);			
			float zfSpeed = zspeed -(int)ceil( movementSpeed)/2;	
			moveDir = D3DXVECTOR3(xfSpeed, 0, zfSpeed);
			i = 999;
			CheckFutureCollision(attempts);
		}
		
	}
}

void Enemy::CheckShooting(float dist, float deltaTime)
{

	PxExtendedVec3 tpPos = physics->player->getPosition();
	PxVec3 pPos = playerPos;
	PxVec3 ori = actor->getGlobalPose().p;
	PxVec3 dir = pPos - ori;
	dir.normalize();
	int numHits = 0;
	PxRaycastHit* hit = physics->RaycastMultiple(ori,dir,dist,&numHits, PxQueryFlag::eSTATIC | PxQueryFlag::eDYNAMIC);
	for(unsigned int i = 1; i < numHits; i++)
	{
		if(hit[i-1].actor == actor && hit[i].actor == physics->player->getActor())
		{
			sawPlayer = true;
			lastTimeShot+=deltaTime;
			if(lastTimeShot > shootDelay)
			{
				obj->PlayAnimation("Shoot",true);
				moveDir *= 0.1f;
				if(obj->GetCurrentAnim()->doAction)
				{
					CreateBullet(ori+dir*2,dir);
					currentMoveTime = 9999.0f;
					lastTimeShot = 0;
					resources->GetSoundHandler()->PlayWaveFile("piew");
					obj->GetCurrentAnim()->doAction = false;
				}
			}
		}
	}
}

        int RayCastManagerImpl::CastRayAgainstCharacterController(const DirectX::XMFLOAT3& p_origin, const DirectX::XMFLOAT3& p_direction, const float& p_range)
        {
            if(p_range <= 0.0f)
            {
                // TODOKO log error
                return -1;
            }
            // Cast directx things to physx
            PxVec3 origin = PxVec3(p_origin.x, p_origin.y, p_origin.z);
            PxVec3 direction = PxVec3(p_direction.x, p_direction.y, p_direction.z);
            direction.normalize();
            PxRaycastBuffer hit; // Used to save the hit
            // casts a ray vs the physics scene. various hit information is saved in the hit variable. For now we only care about the object we hit
            // eMESH_BOTH_SIDES specifices that no culling should be used, shouldnt need to be there but let's be safe
            bool status = m_utils.m_worldScene->raycast(origin, direction, p_range, hit, PxHitFlag::eMESH_BOTH_SIDES);
            // hit.block.position;
            if(!status && !hit.hasBlock)
            {
                // No hit detected, return -1 TODOKO Maybee i should return something better?
                return -1;
            }

            // Now comes the difficult task of checking vs character controllers
            unordered_map<PxController*, int> idsByCharacterController = m_utils.m_characterControlManager->GetIdsByControllers();
            for(auto pairs : idsByCharacterController) // Loop through every pair in the list
            {
                if(pairs.first->getActor() == hit.block.actor) // The first part contains the actor pointer
                {
                    return pairs.second; // If this is true we found a hit vs character controller, second contains ID
                }
            }

            return -1;
        }

        int RayCastManagerImpl::CastSweep(const XMFLOAT3& p_origin, XMFLOAT3& p_direction, float p_width, const float& p_range, int& o_flag)
        {
            if(p_range <= 0.0f)
            {
                cout << "Physcs. Raytracer. Sweep. Range of sweep was zero or below" << endl;
                return -1;
            }
            // Cast directx things to physx
            PxVec3 origin = PxVec3(p_origin.x, p_origin.y, p_origin.z);
            PxVec3 direction = PxVec3(p_direction.x, p_direction.y, p_direction.z);
            direction.normalize();
            PxSweepBuffer hit; // Used to save the hit
            /// Paramters for the sweep
            // PxGeometry* geometry
            bool status = m_utils.m_worldScene->sweep(PxSphereGeometry(p_width), PxTransform(origin), direction, p_range, hit, PxHitFlag::eMESH_BOTH_SIDES);
            // hit.block.position;
            if(!status && !hit.hasBlock)
            {
                // No hit detected, return -1 TODOKO Maybee i should return something better?
                return -1;
            }

            // Start with checking static and dynamic rigid bodies
            unordered_map<PxRigidActor*, int> idsByRigidBody = m_utils.m_rigidBodyManager->GetIDsByBodies();
            if(idsByRigidBody.find(hit.block.actor) != idsByRigidBody.end())
            {
                PxRigidActor* actorAsRigic = (PxRigidActor*)hit.block.actor;
                if(actorAsRigic->isRigidDynamic())
                {
                    PxRigidDynamic* actorsAsDynamic = (PxRigidDynamic*)hit.block.actor;
                    if(actorsAsDynamic->getRigidBodyFlags() & PxRigidBodyFlag::eKINEMATIC)
                    {
                        o_flag = 0;
                    }
                }
                else if(actorAsRigic->isRigidStatic())
                {
                    o_flag = 3;
                }
                return idsByRigidBody.find(hit.block.actor)->second;
            }
            else
            {
                // Nothing
            }
            // Now comes the difficult task of checking vs character controllers
            unordered_map<PxController*, int> idsByCharacterController = m_utils.m_characterControlManager->GetIdsByControllers();
            for(auto pairs : idsByCharacterController) // Loop through every pair in the list
            {
                if(pairs.first->getActor() == hit.block.actor) // The first part contains the actor pointer
                {
                    o_flag = 1;
                    return pairs.second; // If this is true we found a hit vs character controller, second contains ID
                }
            }

            return -1;
        }

// -------------------------------------------------------------------------------------
void PolygonTriangulator::importPoints(const PxVec3 *points, int numPoints, const int *indices, PxVec3 *planeNormal, bool &isConvex)
{
	// find projection 3d -> 2d;
	PxVec3 n;

	isConvex = true;

	if (planeNormal) 
		n = *planeNormal;
	else {
		PX_ASSERT(numPoints >= 3);
		n = PxVec3(0.0f, 0.0f, 0.0f);

		for (int i = 1; i < numPoints-1; i++) {
			int i0 = 0;
			int i1 = i;
			int i2 = i+1;
			if (indices) {
				i0 = indices[i0];
				i1 = indices[i1];
				i2 = indices[i2];
			}
			const PxVec3 &p0 = points[i0];
			const PxVec3 &p1 = points[i1];
			const PxVec3 &p2 = points[i2];
			PxVec3 ni = (p1-p0).cross(p2-p0);
			if (i > 1 && ni.dot(n) < 0.0f)
				isConvex = false;
			n += ni;
		}
	}

	n.normalize();
	PxVec3 t0,t1;

	if (fabs(n.x) < fabs(n.y) && fabs(n.x) < fabs(n.z))
		t0 = PxVec3(1.0f, 0.0f, 0.0f);
	else if (fabs(n.y) < fabs(n.z))
		t0 = PxVec3(0.0f, 1.0f, 0.0f);
	else
		t0 = PxVec3(0.0f, 0.0f, 1.0f);
	t1 = n.cross(t0);
	t1.normalize();
	t0 = t1.cross(n);
	
	mPoints.resize((uint32_t)numPoints);
	if (indices == NULL) {
		for (uint32_t i = 0; i < (uint32_t)numPoints; i++) 
			mPoints[i] = PxVec3(points[i].dot(t0), points[i].dot(t1), 0.0f);
	}
	else {
		for (uint32_t i = 0; i < (uint32_t)numPoints; i++) {
			const PxVec3 &p = points[(uint32_t)indices[i]];
			mPoints[i] = PxVec3(p.dot(t0), p.dot(t1), 0.0f);
		}
	}
}

PxTransform Camera::getTransform() const
{
	PxVec3 viewY = mDir.cross(PxVec3(0, 1, 0));

	if (viewY.normalize() < 1e-6f)
		return PxTransform(mEye);

	PxMat33 m(mDir.cross(viewY), viewY, -mDir);
	return PxTransform(mEye, PxQuat(m));
}

static void normalToTangents(const PxVec3& n, PxVec3& t1, PxVec3& t2)
{
	const PxReal m_sqrt1_2 = PxReal(0.7071067811865475244008443621048490);
	if(fabsf(n.z) > m_sqrt1_2)
	{
		const PxReal a = n.y*n.y + n.z*n.z;
		const PxReal k = PxReal(1.0)/PxSqrt(a);
		t1 = PxVec3(0,-n.z*k,n.y*k);
		t2 = PxVec3(a*k,-n.x*t1.z,n.x*t1.y);
	}
	else 
	{
		const PxReal a = n.x*n.x + n.y*n.y;
		const PxReal k = PxReal(1.0)/PxSqrt(a);
		t1 = PxVec3(-n.y*k,n.x*k,0);
		t2 = PxVec3(-n.z*t1.y,n.z*t1.x,a*k);
	}
	t1.normalize();
	t2.normalize();
}

void RandomVelocities(PhysXObject* object, int powerMax, int seedMultiplier)
{
    PxVec3 dir = CreateRandomVector(powerMax, seedMultiplier);

    srand((time(NULL) * seedMultiplier) + time(NULL));

    int power = rand() % powerMax;

    dir.normalize();

    SetVelocity(dir * power, object);
}

/**
 @brief setting control diagramnode
 @date 2014-03-02
*/
void COrientationEditController::SetControlDiagram(CGenotypeNode *node)
{
	RemoveGenotypeTree(m_sample, m_rootNode);
	m_nodes.clear();
	m_rootNode = NULL;

	vector<CGenotypeNode*> nodes;
	m_genotypeController.GetDiagramsLinkto(node, nodes);
	if (nodes.empty())
		return;

	// Create Phenotype Node
	CGenotypeNode *parentNode = nodes[ 0];
	map<const genotype_parser::SExpr*, CGenotypeNode*> symbols;
	const PxTransform identTm = PxTransform::createIdentity();
	m_rootNode = CreatePhenotypeDiagram(identTm,identTm,identTm, parentNode->m_expr, symbols);

	// Camera Setting
	const PxVec3 parentPos = parentNode->GetWorldTransform().p;
	PxVec3 dir = parentPos - node->GetWorldTransform().p;
	dir.normalize();

	PxVec3 left = PxVec3(0,1,0).cross(dir);
	left.normalize();
	PxVec3 camPos = node->GetWorldTransform().p + (left*3.f) + PxVec3(0,2.5f,0);
	PxVec3 camDir = parentPos - camPos;
	camDir.normalize();
	camPos -= (camDir * .5f);

	m_sample.getCamera().lookAt(camPos, parentPos);
	const PxTransform viewTm = m_sample.getCamera().getViewMatrix();
	m_camera->init(viewTm);

	// select Orientation Control node
	if (CGenotypeNode *selectNode = m_rootNode->GetConnectNode(node->m_name))
	{
		selectNode->SetHighLight(true);
		SelectNode(selectNode);
	}
}

void Jumper::RunningJump::update(float dt)
{
    updateAnimation( dt );
    _clump->getFrame()->getLTM();

    if( _actionTime < _blendTime ) 
    {
        Vector3f dir = _clump->getFrame()->getAt();
        dir.normalize();
        _clump->getFrame()->setPos( _clump->getFrame()->getPos() + dir * dt * _vel );            
        return;
    }

    if( _phActor->isSleeping() )
    {
        // setup physics
        Matrix4f sampleLTM = Jumper::getCollisionFF( _clump )->getFrame()->getLTM();
        _phActor->setGlobalPose(PxTransform(wrap( sampleLTM )));
        _phActor->wakeUp();
        PxVec3 velH = wrap( _clump->getFrame()->getAt() );
        velH.normalize();
        velH *= _vel * 0.01f;
        PxVec3 velV = wrap( _clump->getFrame()->getUp() );
        velV.normalize();
        velV *= 1.5f;
        _phActor->setLinearVelocity( velH + velV + wrap(_clump->getFrame()->getAt() * 600.0f * dt)) ;
        _jumper->initOverburdenCalculator( velH + velV );


    }
    else
    {
        _clump->getFrame()->setMatrix( _matrixConversion->convert( wrap( _phActor->getGlobalPose() ) ) );
    }

    if( _clump->getAnimationController()->isEndOfAnimation( 0 ) )
    {
        _endOfAction = true;
    }
}

/**
 @brief generate triangle index buffer
 @date 2014-01-30
*/
void SampleRenderer::GenerateTriangleFrom3Vector( void *positions, void *normals, PxU32 stride, 
	const PxVec3 &center, const vector<PxU16> &triangle, OUT vector<PxU16> &outTriangle )
{
	const PxVec3 p0 = *(PxVec3*)(((PxU8*)positions) + (stride * triangle[ 0]));
	const PxVec3 p1 = *(PxVec3*)(((PxU8*)positions) + (stride * triangle[ 1]));
	const PxVec3 p2 = *(PxVec3*)(((PxU8*)positions) + (stride * triangle[ 2]));

	PxVec3 faceCenter = (p0 + p1 + p2) / 3.f;
	PxVec3 n = faceCenter - center;
	n.normalize();

	// ccw check
	//
	//            p0
	//          /   \
	//        /        \
	//    p2  ------  p1
	//

	PxVec3 v10 = p1 - p0;
	v10.normalize();
	PxVec3 v20 = p2 - p0;
	v20.normalize();

	PxVec3 crossV = v10.cross(v20);
	crossV.normalize();

	if (n.dot(crossV) >= 0)
	{
		outTriangle.push_back( triangle[ 0] );
		outTriangle.push_back( triangle[ 2] );
		outTriangle.push_back( triangle[ 1] );
	}
	else
	{
		outTriangle.push_back( triangle[ 0] );
		outTriangle.push_back( triangle[ 1] );
		outTriangle.push_back( triangle[ 2] );
	}
}