steering behaviors 转向行为 集群模拟 小结

#define dao 6.28318530718

#pragma label max_force   "Max Factor Force"
#pragma label slow_radius "Seek Slow Radius"
#pragma label acc_weight  "Acceleration weight"

#pragma label pc_radius   "Neighbours Radius"
#pragma label pc_ptnum    "Neighbours number"

#pragma label wander_max  "Wander Max Velocity"
#pragma label sep_weight  "Seperation Weight"
#pragma label co_weight   "Cohesion Weight"
#pragma label ali_weight  "Alignment Weight"

#pragma hint avoid_center invisible

//get target
vector gettarget(int target_handle){
        vector target;
        while(pciterate(target_handle)){
            pcimport(target_handle, "P", target);
        }
        return target;
}


// seek + flee + cohesion + seperation
vector seek(vector target; float slow_radius; float max_force;){
        float seek_power  = fit(length(target - P), 0, slow_radius, 0, 1) * max_force;
        vector seek_force = seek_power * normalize(target - P);
        return seek_force;

}

vector flee(vector target; float slow_radius; float max_force;){
        float flee_power  = -fit(length(target - P), 0, slow_radius*1.5, 1, 0) * max_force;
        vector flee_force = flee_power * normalize(target - P);
        return flee_force;
}

vector wander(vector max_force; float wander_max;){
        //wander force
        //to constrain the steering force to the surface of a sphere located slightly ahead of the character
        //any point on the sphere:   0 < theta < dao ; 0 < beta < dao/2
        // x = r * cos theta * sin beta
        // y = r * sin theta * sin beta
        // z = r * cos beta
        float theta, beta;
        
        theta = fit01(noise(P*37), 0, dao);
        beta  = fit01(noise(P*10+234), 0, dao/2);
        
        vector displacement = set(sin(theta) * cos(beta), sin(theta) * sin(beta), cos(beta));
        vector wander_dir = normalize(2 * normalize(v) + displacement) * wander_max;

        return wander_dir; // * fit(length(v), 0, max_force, 1, 0) ;
}

vector cohesion(int self_handle; float pc_radius; float co_weight){
        //Cohesion
        vector co_target = set(0,0,0);
        while(pciterate(self_handle)){
                vector current_point;
                pcimport(self_handle, "P", current_point);
                co_target += current_point; 
        }
        vector co_force = normalize(co_target - P) * pow(fit(length(co_target - P), 0, pc_radius, 1, 0), 2) * co_weight;
        return co_force;
}

vector seperation(int self_handle; float pc_radius; float sep_weight){
        //Seperation
        vector sep_force = set(0,0,0);
        while(pciterate(self_handle)){
                vector current_point ;
                pcimport(self_handle, "P", current_point);
                sep_force += normalize(P - current_point) * pow(fit(length(current_point - P), 0, pc_radius, 1, 0), 2);
        }
        sep_force = normalize(sep_force) * sep_weight;
        return sep_force;
}

vector alignment(int self_handle; float pc_radius; float ali_weight;){
        //Alignment
        vector average_dirction = set(0,0,0);
    while(pciterate(self_handle)){
            vector current_velocity;
            vector current_point;
            pcimport(self_handle, "v", current_velocity);
            pcimport(self_handle, "P", current_point);
            average_dirction += normalize(current_velocity) * pow(fit(length(current_point - P), 0, pc_radius, 1, 0), 2);
    }
    average_dirction = normalize(average_dirction) * ali_weight;
    return average_dirction;
}

vector obstacle_avoidance(vector avoid_center; float avoid_radius; float avoid_weight;){
        vector hit_position;
        float p_u, p_v;
        int hit_prim = intersect(2, P, normalize(avoid_center - P) * avoid_radius, hit_position, p_u, p_v);
        if(hit_prim != -1){
                vector turn_force, turn_direction, second_direction;
                second_direction = prim_normal(2, hit_prim, p_u, p_v);
                turn_direction = cross((hit_position - P), second_direction);
                turn_force = normalize(turn_direction) * fit(length(hit_position - P), 0, avoid_radius*10, 1, 0) * avoid_weight;
                return turn_force;
        }else{return 0;}
}

sop
steering(
                // max velocity 
                float max_force   = 1.0;
                // the area radius around target, 
                //when charactor get in this area, the the seek_power will drop down
                float slow_radius = 1.0;
                //acceleration scale for all charactors
                float acc_weight      = 0.3;
                //point cloud radius
                float pc_radius   = 0.5;
                //point cloud point number
                int pc_ptnum      = 10;
                //wander max
                float wander_max  = 0.02;
                //seperation weight
                float sep_weight  = 0.2;
                //cohesion weight
                float co_weight   = 1.0;
                //alignment weight
                float ali_weight  = 1.0;
                //obstacle avoidance weight
                float avoid_weight  = 1.0;
                //obstacle avoidance radius
                float avoid_radius  = 1.0;
                // avoid target centroid
                export vector avoid_center = 0;
                )
{
        // get the target location
        vector target;
        int target_handle;
        target_handle = pcopen(1, "P", P, 99999, 1);
        target = gettarget(target_handle);

        // open the point cloud
        int self_handle;
        self_handle = pcopen(geoself(), "P", P, pc_radius, pc_ptnum);

        // desired_v is the force that guides the character towards its target using the shortest path possible
        // we add wander_force as an extra factor that will effect desired_v
        vector desired_v;
        vector steering;

        //define every factor forces
        vector seek_force;
        vector flee_force;
        vector wander_force;
        vector co_force;
        vector sep_force;
        vector ali_force;
        vector avoid_force;

        //get each forces
        seek_force   = seek(target, slow_radius, max_force);
        flee_force   = flee(target, slow_radius, max_force);
        wander_force = wander(max_force, wander_max);
        co_force     = cohesion(self_handle, pc_radius, co_weight);
        sep_force    = seperation(self_handle, pc_radius, sep_weight);
        ali_force    = alignment(self_handle, pc_radius, ali_weight);
        avoid_force  = obstacle_avoidance(avoid_center, avoid_radius, avoid_weight);

        // desired_v = seek force + flee force + wander force + seperation force
        desired_v  = seek_force + flee_force + wander_force + co_force + sep_force + ali_force + avoid_force;

        steering   = desired_v - v;
        steering   = steering * acc_weight;
        
        v += steering;
        P += v;
}