C++ Fvector::set_length方法代码示例

//-----------------------------------------------------------------------------
// Name: collideWithWorld()
// Desc: Recursive part of the collision response. This function is the
//       one who actually calls the collision check on the meshes
//-----------------------------------------------------------------------------
Fvector msimulator_CollideWithWorld(SCollisionData& cl, Fvector position, Fvector velocity, WORD cnt)
{
    //
    msimulator_ResolveStuck(cl,position);

    // do we need to worry ?
//	if (fsimilar(position.x,target.x,EPS_L)&&fsimilar(position.z,target.z,EPS_L))
    if (velocity.magnitude()<EPS_L)
    {
        cl.vVelocity.set(0,0,0);
        return position;
    }
    if (cnt>psCollideActDepth)				return cl.vLastSafePosition;

    Fvector ret_pos;
    Fvector destinationPoint;
    destinationPoint.add(position,velocity);

    // reset the collision package we send to the mesh
    cl.vVelocity.set	(velocity);
    cl.vSourcePoint.set	(position);
    cl.bFoundCollision	= FALSE;
    cl.bStuck			= FALSE;
    cl.fNearestDistance	= -1;

    // Check collision
    msimulator_CheckCollision	(cl);

    // check return value here, and possibly call recursively
    if (cl.bFoundCollision == FALSE  && !cl.bStuck)
    {
        // if no collision move very close to the desired destination.
        float l = velocity.magnitude();
        Fvector V;
        V.mul(velocity,(l-cl_epsilon)/l);

        // return the final position
        ret_pos.add(position,V);

        // update the last safe position for future error recovery
        cl.vLastSafePosition.set(ret_pos);
        return ret_pos;
    } else {
        // There was a collision
        // OK, first task is to move close to where we hit something :
        Fvector newSourcePoint;

        // If we are stuck, we just back up to last safe position
        if (cl.bStuck) {
            return cl.vLastSafePosition;
        }

        // only update if we are not already very close
        if (cl.fNearestDistance >= cl_epsilon) {
            Fvector V;
            V.set(velocity);
            V.set_length(cl.fNearestDistance-cl_epsilon);
            newSourcePoint.add(cl.vSourcePoint,V);
        } else {
            newSourcePoint.set(cl.vSourcePoint);
        }

        // Now we must calculate the sliding plane
        Fvector slidePlaneOrigin;
        slidePlaneOrigin.set(cl.vNearestPolygonIntersectionPoint);
        Fvector slidePlaneNormal;
        slidePlaneNormal.sub(newSourcePoint,cl.vNearestPolygonIntersectionPoint);

        // We now project the destination point onto the sliding plane
        float l = intersectRayPlane(destinationPoint, slidePlaneNormal, slidePlaneOrigin, slidePlaneNormal);

        // We can now calculate a _new destination point on the sliding plane
        Fvector newDestinationPoint;
        newDestinationPoint.mad(destinationPoint,slidePlaneNormal,l);

        // Generate the slide xr_vector, which will become our _new velocity xr_vector
        // for the next iteration
        Fvector newVelocityVector;
        newVelocityVector.sub(newDestinationPoint, cl.vNearestPolygonIntersectionPoint);

        // now we recursively call the function with the _new position and velocity
        cl.vLastSafePosition.set(position);
        return msimulator_CollideWithWorld(cl, newSourcePoint, newVelocityVector,cnt+1);
    }
}

void CBaseMonster::update_pos_by_grouping_behaviour ()
{
	if ( !m_grouping_behaviour )
	{
		return;
	}

	Fvector acc = get_steer_manager()->calc_acceleration();

	acc.y = 0; // remove vertical component

	if ( !m_last_grouping_behaviour_update_tick )
	{
		m_last_grouping_behaviour_update_tick = Device.dwTimeGlobal;
	}	

	const float dt = 0.001f * (Device.dwTimeGlobal - m_last_grouping_behaviour_update_tick);
	
	m_last_grouping_behaviour_update_tick = Device.dwTimeGlobal;

	const Fvector old_pos  = Position();
	Fvector       offs     = acc*dt;
	const float   offs_mag = magnitude(offs);

	if ( offs_mag < 0.000001f )
	{
		// too little force applied, ignore it and save cpu
		return;
	}

	// this control maximum offset
	// higher values allow stronger forces, but can lead to jingling
	const float max_offs = 0.005f;
	if ( offs_mag > max_offs )
	{
		offs.set_length(0.005f);
	}

	Fvector   new_pos    = old_pos + offs;


	const u32 old_vertex = ai_location().level_vertex_id();
	u32       new_vertex = ai().level_graph().check_position_in_direction(old_vertex, old_pos, new_pos);

	if ( !ai().level_graph().valid_vertex_id(new_vertex) )
	{
		// aiming out of ai-map, ignore
		return;
	}

	// use physics simulation to slide along obstacles
	character_physics_support()->movement()->VirtualMoveTo(new_pos, new_pos);

	if ( !ai().level_graph().valid_vertex_position(new_pos) )
	{
		// aiming out of ai-map, ignore
		return;
	}

	new_vertex = ai().level_graph().check_position_in_direction(old_vertex, old_pos, new_pos);

	if ( !ai().level_graph().valid_vertex_id(new_vertex) )
	{
		return;
	}

	// finally, new position is valid on the ai-map, we can use it
	character_physics_support()->movement()->SetPosition(new_pos);
	Position() = new_pos;
	ai_location().level_vertex(new_vertex);
}