821 lines
24 KiB
C++
821 lines
24 KiB
C++
#include <cassert>
|
|
#include <cstdio>
|
|
#include <core/list.h>
|
|
#include <scene/3d/spatial.h>
|
|
#include <scene/3d/physics_body.h>
|
|
#include <scene/3d/immediate_geometry.h>
|
|
#include <scene/3d/vehicle_body.h>
|
|
#include <scene/animation/animation_tree.h>
|
|
#include <scene/animation/animation_node_state_machine.h>
|
|
#include <scene/resources/mesh.h>
|
|
#include <modules/detour/crowd.h>
|
|
#include "smart_object.h"
|
|
#include "characters.h"
|
|
|
|
|
|
Characters_::Characters_() : query(memnew(DetourNavigationQuery)),
|
|
initialized(false), debug(NULL),
|
|
crowd(NULL)
|
|
{
|
|
smm = NULL;
|
|
}
|
|
AnimationTree *Characters_::get_animation_tree(const Node *npc) const
|
|
{
|
|
if (!npc->has_meta("animation_tree"))
|
|
return NULL;
|
|
Object * obj = npc->get_meta("animation_tree");
|
|
if (!obj)
|
|
return NULL;
|
|
AnimationTree *result = Object::cast_to<AnimationTree>(obj);
|
|
return result;
|
|
}
|
|
bool Characters_::handle_cmdq(Node *npc)
|
|
{
|
|
Array cmds, cmd;
|
|
bool handled = false;
|
|
if (npc->has_meta("cmdqueue"))
|
|
cmds = npc->get_meta("cmdqueue");
|
|
AnimationTree *anim = get_animation_tree(npc);
|
|
if (cmds.size() > 0) {
|
|
cmd = cmds[0];
|
|
if (cmd[0] == "anim_state") {
|
|
animation_node_travel(npc, cmd[1]);
|
|
cmds.pop_front();
|
|
handled = true;
|
|
} else if (cmd[0] == "anim_param") {
|
|
String p = "parameters/";
|
|
p += String(cmd[1]);
|
|
anim->set(p, cmd[2]);
|
|
cmds.pop_front();
|
|
handled = true;
|
|
} else if (cmd[0] == "delay") {
|
|
int d = cmd[1];
|
|
cmds.pop_front();
|
|
if (d > 0) {
|
|
d--;
|
|
Array ncmd;
|
|
ncmd.push_back("delay");
|
|
ncmd.push_back(d);
|
|
cmds.push_front(ncmd);
|
|
}
|
|
handled = true;
|
|
} else if (cmd[0] == "jump_to_obj") {
|
|
Object *obj = cmd[1];
|
|
Spatial *sp = Object::cast_to<Spatial>(obj);
|
|
Spatial *npc_sp = Object::cast_to<Spatial>(npc);
|
|
Transform g = sp->get_global_transform();
|
|
npc_sp->set_global_transform(g);
|
|
Transform orientation = npc->get_meta("orientation");
|
|
orientation.basis = g.basis;
|
|
npc->set_meta("orientation", orientation);
|
|
cmds.pop_front();
|
|
handled = true;
|
|
} else if (cmd[0] == "jump_to_obj_rot") {
|
|
Object *obj = cmd[1];
|
|
Spatial *sp = Object::cast_to<Spatial>(obj);
|
|
Spatial *npc_sp = Object::cast_to<Spatial>(npc);
|
|
Transform xform;
|
|
Transform g = sp->get_global_transform();
|
|
xform.origin = g.origin;
|
|
xform.basis = g.basis.rotated(Vector3(0, 1, 0), Math_PI);
|
|
npc_sp->set_global_transform(xform);
|
|
Transform orientation = npc->get_meta("orientation");
|
|
orientation.basis = xform.basis;
|
|
npc->set_meta("orientation", orientation);
|
|
cmds.pop_front();
|
|
handled = true;
|
|
}
|
|
}
|
|
|
|
if (cmds.size() == 0 && npc->has_meta("cmdqueue"))
|
|
npc->remove_meta("cmdqueue");
|
|
else if (cmds.size() > 0 && handled)
|
|
npc->set_meta("cmdqueue", cmds);
|
|
return handled;
|
|
}
|
|
String Characters_::get_animation_node(const Node *npc)
|
|
{
|
|
AnimationTree *anim_tree = get_animation_tree(npc);
|
|
if (!anim_tree)
|
|
return "";
|
|
Ref<AnimationNodeStateMachinePlayback> playback;
|
|
playback = anim_tree->get("parameters/state/playback");
|
|
if (!playback.ptr())
|
|
return "";
|
|
return playback->get_current_node();
|
|
}
|
|
void Characters_::animation_node_travel(Node *npc, const String &anim)
|
|
{
|
|
AnimationTree *anim_tree = get_animation_tree(npc);
|
|
if (!anim_tree)
|
|
return;
|
|
Ref<AnimationNodeStateMachinePlayback> playback;
|
|
playback = anim_tree->get("parameters/state/playback");
|
|
if (!playback.ptr())
|
|
return;
|
|
playback->travel(anim);
|
|
}
|
|
void Characters_::set_walk_speed(Node *npc, float speed, float strafe)
|
|
{
|
|
AnimationTree *anim_tree = get_animation_tree(npc);
|
|
if (!anim_tree)
|
|
return;
|
|
Vector2 spd(speed, strafe);
|
|
anim_tree->set("parameters/state/locomotion/loc/blend_position", spd);
|
|
}
|
|
float Characters_::get_walk_speed(const Node *npc) const
|
|
{
|
|
AnimationTree *anim_tree = get_animation_tree(npc);
|
|
if (!anim_tree)
|
|
return 0.0f;
|
|
Vector2 spd = anim_tree->get("parameters/state/locomotion/loc/blend_position");
|
|
return spd.x;
|
|
}
|
|
void Characters_::rotate_to_agent(Spatial *obj)
|
|
{
|
|
if (!obj->has_meta("agent_id"))
|
|
return;
|
|
Transform orientation = obj->get_meta("orientation");
|
|
#if 0
|
|
if (get_animation_node(obj).begins_with("turn_"))
|
|
return;
|
|
#endif
|
|
Vector3 agent_velocity = obj->get_meta("agent_velocity");
|
|
Vector3 agent_position = obj->get_meta("agent_position");
|
|
Transform g = obj->get_global_transform();
|
|
Vector3 to = agent_position - g.origin + agent_velocity;
|
|
Vector3 dir = orientation.xform(Vector3(0.0f, 0.0f, -1.0f));
|
|
to.y = 0.0f;
|
|
Vector2 to2(to.x, to.z);
|
|
Vector2 dir2(dir.x, dir.z);
|
|
float angle = dir2.angle_to(to2);
|
|
if (fabs(angle) < Math_PI / 8.0f || to.length_squared() < 1.9f * 1.9f) {
|
|
if (get_animation_node(obj).begins_with("turn_"))
|
|
animation_node_travel(obj, "locomotion");
|
|
Transform rot = Transform().looking_at(to, Vector3(0.0f, 1.0f, 0.0f));
|
|
orientation.basis = rot.basis;
|
|
obj->set_meta("orientation", orientation);
|
|
g.basis = orientation.basis;
|
|
obj->set_global_transform(g);
|
|
} else {
|
|
if (get_animation_node(obj).begins_with("turn_"))
|
|
return;
|
|
Vector3 sk = orientation.xform_inv(to);
|
|
if (sk.z > 1.9f)
|
|
animation_node_travel(obj, "turn_left");
|
|
/* CCW */
|
|
else if (sk.x > 0.0f)
|
|
animation_node_travel(obj, "turn_left");
|
|
/* CW */
|
|
else if (sk.x < 0.0f)
|
|
animation_node_travel(obj, "turn_right");
|
|
}
|
|
}
|
|
void Characters_::speed_to_agent(Spatial *obj)
|
|
{
|
|
float delta = get_physics_process_delta_time();
|
|
float cur_speed = get_walk_speed(obj);
|
|
float new_speed;
|
|
Vector3 agent_velocity = obj->get_meta("agent_velocity");
|
|
Vector3 agent_position = obj->get_meta("agent_position");
|
|
int agent_id = obj->get_meta("agent_id");
|
|
bool update_speed = false;
|
|
Transform g = obj->get_global_transform();
|
|
#if 0
|
|
KinematicBody *kb = Object::cast_to<KinematicBody>(obj);
|
|
#endif
|
|
if (obj->has_meta("cur_speed")) {
|
|
float tmp = obj->get_meta("cur_speed");
|
|
if (cur_speed != tmp) {
|
|
#if 0
|
|
printf("something is very wrong\n");
|
|
#endif
|
|
cur_speed = cur_speed + (tmp - cur_speed) * delta;
|
|
set_walk_speed(obj, cur_speed, 0.0f);
|
|
}
|
|
}
|
|
if (agent_position.distance_squared_to(g.origin) > 0.0f) {
|
|
agent_velocity = agent_position - g.origin + agent_velocity;
|
|
Vector3 apos = g.xform_inv(agent_position);
|
|
float dinc = -apos.z;
|
|
float xinc = apos.x;
|
|
// printf("position mismatch %f\n", dinc);
|
|
if (fabs(dinc) > 0.2f || fabs(xinc) > 0.2f)
|
|
crowd->reset_agent(agent_id);
|
|
Vector3 v = agent_velocity;
|
|
v.y = 0.0f;
|
|
float spd = v.length();
|
|
#if 0
|
|
cur_speed = get_walk_speed(obj);
|
|
if (obj->has_meta("cur_speed"))
|
|
cur_speed = obj->get_meta("cur_speed");
|
|
#endif
|
|
|
|
if (spd > 40.0f)
|
|
new_speed = 0.5f;
|
|
else if (spd > 0.0f)
|
|
new_speed = 0.1f + 0.4f * (spd / 60.0f);
|
|
else
|
|
new_speed = 0.0f;
|
|
#if 0
|
|
if (dinc > 0.15f)
|
|
new_speed += 0.1f * delta;
|
|
if (dinc < -0.15f)
|
|
new_speed -= 0.1f * delta;
|
|
#endif
|
|
if (new_speed > 0.0f)
|
|
new_speed = cur_speed +
|
|
(new_speed - cur_speed) * delta;
|
|
new_speed = CLAMP(new_speed, 0.0f, 1.0f);
|
|
if (new_speed != cur_speed) {
|
|
#if 0
|
|
printf("cur_speed = %f %f spd = %f\n", cur_speed, dinc, spd);
|
|
printf("new speed = %f cur_speed = %f\n", new_speed, cur_speed);
|
|
#endif
|
|
if (new_speed == 0.0f && cur_speed > 0.0f) {
|
|
animation_node_travel(obj, "stop_walking");
|
|
printf("stopping 1\n");
|
|
obj->set_meta("cur_speed", 0.0f);
|
|
cur_speed = new_speed;
|
|
update_speed = true;
|
|
}
|
|
else if (cur_speed == 0.0f && new_speed > 0.0f) {
|
|
animation_node_travel(obj, "start_walking");
|
|
printf("starting 1\n");
|
|
obj->set_meta("cur_speed", new_speed);
|
|
cur_speed = new_speed;
|
|
update_speed = true;
|
|
} else if (cur_speed >= 0.0f && new_speed > 0.0f) {
|
|
obj->set_meta("cur_speed", new_speed);
|
|
cur_speed = new_speed;
|
|
update_speed = true;
|
|
}
|
|
}
|
|
// printf("cur_speed = %f speed = %f new_speed = %f\n", cur_speed, get_walk_speed(obj), new_speed);
|
|
if (new_speed > 0.0f) {
|
|
if (dinc > 0.15f) {
|
|
new_speed = CLAMP(new_speed + delta * 2.5f, 0.1f, 1.0f);
|
|
update_speed = true;
|
|
} else if (dinc < -0.15f) {
|
|
new_speed = CLAMP(new_speed - delta * 2.5f, 0.1f, 1.0f);
|
|
update_speed = true;
|
|
}
|
|
}
|
|
}
|
|
if (update_speed)
|
|
set_walk_speed(obj, new_speed, 0.0f);
|
|
if (cur_speed > 0.0f)
|
|
assert(get_walk_speed(obj) > 0.0f);
|
|
}
|
|
bool Characters_::has_arrived(Object *obj)
|
|
{
|
|
Spatial *sp = Object::cast_to<Spatial>(obj);
|
|
if (!obj->has_meta("agent_id"))
|
|
return false;
|
|
if (!obj->has_meta("_target"))
|
|
return false;
|
|
int agent_id = obj->get_meta("agent_id");
|
|
bool crowd_arrived = crowd->has_arrived(agent_id);
|
|
if (crowd_arrived) {
|
|
Vector3 cpos = obj->get_meta("agent_position");
|
|
Vector3 spos = sp->get_global_transform().origin;
|
|
if (cpos.distance_squared_to(spos) <
|
|
arrive_precision * arrive_precision)
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
void Characters_::update_arrived(Object *obj)
|
|
{
|
|
Spatial *sp = Object::cast_to<Spatial>(obj);
|
|
int agent_id = obj->get_meta("agent_id");
|
|
if (obj->has_meta("climb"))
|
|
obj->remove_meta("climb");
|
|
crowd->clear_agent_target(agent_id);
|
|
if (!sp)
|
|
return;
|
|
printf("stopping 2\n");
|
|
set_walk_speed(sp, 0.0f, 0.0f);
|
|
obj->set_meta("cur_speed", 0.0f);
|
|
animation_node_travel(sp, "stop_walking");
|
|
if (obj->has_meta("_target_jumpto")) {
|
|
Transform xform = obj->get_meta("_target_jumpto");
|
|
sp->set_global_transform(xform);
|
|
obj->remove_meta("_target_jumpto");
|
|
}
|
|
if (obj->has_meta("_target_animation")) {
|
|
String anim = obj->get_meta("_target_animation");
|
|
animation_node_travel(sp, anim);
|
|
obj->remove_meta("_target_animation");
|
|
}
|
|
if (obj->has_meta("_target_cmdq")) {
|
|
Array cmds;
|
|
if (obj->has_meta("cmdqueue"))
|
|
cmds = obj->get_meta("cmdqueue");
|
|
Array tcmds = obj->get_meta("_target_cmdq");
|
|
cmds.append_array(tcmds);
|
|
obj->set_meta("cmdqueue", cmds);
|
|
obj->remove_meta("_target_cmdq");
|
|
}
|
|
}
|
|
void Characters_::character_physics(Object *obj)
|
|
{
|
|
float delta = get_physics_process_delta_time();
|
|
Vector3 h_velocity, velocity;
|
|
AnimationTree *animtree;
|
|
Transform orientation, root_motion, g;
|
|
Object *tmp;
|
|
|
|
tmp = obj->get_meta("animation_tree");
|
|
animtree = Object::cast_to<AnimationTree>(tmp);
|
|
|
|
orientation = obj->get_meta("orientation");
|
|
root_motion = animtree->get_root_motion_transform();
|
|
orientation *= root_motion;
|
|
h_velocity = orientation.origin / delta;
|
|
velocity = h_velocity;
|
|
KinematicBody *kb = Object::cast_to<KinematicBody>(obj);
|
|
if (kb) {
|
|
bool go = false;
|
|
if (!obj->has_meta("vehicle") && !obj->has_meta("cmdqueue") /*&& !obj->has_meta("smart_object")*/) {
|
|
go = true;
|
|
} else if (obj->has_meta("cmdqueue") && obj->has_meta("cmdq_walk")) {
|
|
go = true;
|
|
} else if (obj->has_meta("cmdqueue") && obj->has_meta("climb")) {
|
|
go = true;
|
|
}
|
|
if (!kb->is_on_floor() && !obj->has_meta("climb"))
|
|
velocity += Vector3(0.0f, -9.8f, 0.0f);
|
|
if (go)
|
|
velocity = kb->move_and_slide(velocity, Vector3(0.0f, 1.0f, 0.0f), true, 4, 0.785f, false);
|
|
}
|
|
orientation.origin = Vector3();
|
|
orientation = orientation.orthonormalized();
|
|
obj->set_meta("orientation", orientation);
|
|
Spatial *sp = Object::cast_to<Spatial>(obj);
|
|
if (sp) {
|
|
g = sp->get_global_transform();
|
|
g.basis = orientation.basis;
|
|
sp->set_global_transform(g);
|
|
|
|
if (obj->has_meta("vehicle")) {
|
|
tmp = obj->get_meta("vehicle");
|
|
VehicleBody *vehicle = Object::cast_to<VehicleBody>(tmp);
|
|
if (obj->has_meta("vehicle_offset")) {
|
|
Transform xform = obj->get_meta("vehicle_offset");
|
|
g = vehicle->get_global_transform() * xform;
|
|
sp->set_global_transform(g);
|
|
}
|
|
}
|
|
}
|
|
Node *node = Object::cast_to<Node>(obj);
|
|
if (node) {
|
|
if (node->has_node(String("blackboard"))) {
|
|
Node *bb = node->get_node(String("blackboard"));
|
|
bb->call("set", "velocity", velocity);
|
|
}
|
|
}
|
|
if (!crowd)
|
|
return;
|
|
if (obj->has_meta("smart_object"))
|
|
return;
|
|
if (obj->has_meta("cmdqueue") && !obj->has_meta("cmdq_walk"))
|
|
return;
|
|
/* agent creation stuff */
|
|
if (!obj->has_meta("agent_id")) {
|
|
float agent_data_f[] = {0.35f, 1.5f, 1.8f, 40.0f};
|
|
PoolVector<float> agent_data;
|
|
agent_data.resize(4);
|
|
memcpy(agent_data.write().ptr(), agent_data_f, sizeof(agent_data_f));
|
|
crowd->add_agent(obj, 0, false, agent_data);
|
|
}
|
|
if (!obj->has_meta("agent_id"))
|
|
return;
|
|
if (!node)
|
|
return;
|
|
if (!obj->has_meta("_target"))
|
|
return;
|
|
float fwd_probe = forward_probe(node, 0.5f, 0.05f, 1.9f, 0.2f);
|
|
bool climb = false;
|
|
bool bumped = false;
|
|
if (obj->has_meta("bumped")) {
|
|
bumped = true;
|
|
float remaining = obj->get_meta("bumped");
|
|
remaining -= delta;
|
|
if (remaining >= 0.0f)
|
|
obj->set_meta("bumped", remaining);
|
|
else
|
|
obj->remove_meta("bumped");
|
|
}
|
|
if (fwd_probe < 0.1f) {
|
|
if (obj->has_meta("climb"))
|
|
climb = true;
|
|
if (get_animation_node(node) != "climb1")
|
|
climb = false;
|
|
} else if (fwd_probe < 1.0f)
|
|
climb = true;
|
|
else if (fwd_probe >= 1.0f && !bumped) {
|
|
int agent_id = obj->get_meta("agent_id");
|
|
printf("bumped into wall %f\n", fwd_probe);
|
|
bumped = true;
|
|
printf("stopping 3\n");
|
|
set_walk_speed(node, 0.0f, 0.0f);
|
|
obj->set_meta("bumped", 3.0f);
|
|
obj->set_meta("cur_speed", 0.0f);
|
|
animation_node_travel(node, "stop_walking");
|
|
crowd->reset_agent(agent_id);
|
|
}
|
|
if (climb && !obj->has_meta("climb")) {
|
|
animation_node_travel(node, "climb1");
|
|
obj->set_meta("climb", true);
|
|
} else if (!climb && obj->has_meta("climb")) {
|
|
animation_node_travel(node, "locomotion");
|
|
obj->remove_meta("climb");
|
|
}
|
|
if (climb)
|
|
return;
|
|
/* agent stuff */
|
|
Vector3 agent_velocity = obj->get_meta("agent_velocity");
|
|
Vector3 agent_position = obj->get_meta("agent_position");
|
|
g = sp->get_global_transform();
|
|
if (!bumped)
|
|
speed_to_agent(sp);
|
|
if (((agent_position - g.origin) * delta + agent_velocity).length() == 0.0f) {
|
|
float wspd = obj->get_meta("cur_speed");
|
|
if (wspd >= 0.0f) {
|
|
printf("stopping 4\n");
|
|
set_walk_speed(node, 0.0f, 0.0f);
|
|
animation_node_travel(node, "stop_walking");
|
|
obj->set_meta("cur_speed", 0.0f);
|
|
}
|
|
} else
|
|
rotate_to_agent(sp);
|
|
if (has_arrived(obj)) {
|
|
printf("ARRIVED\n");
|
|
Vector3 where;
|
|
update_arrived(obj);
|
|
bool smart = false;
|
|
SmartObject *sm = NULL;
|
|
if (obj->has_meta("_target_node")) {
|
|
Object *o = ObjectDB::get_instance(obj->get_meta("_target_node"));
|
|
if (o) {
|
|
sm = Object::cast_to<SmartObject>(o);
|
|
if (sm)
|
|
smart = true;
|
|
}
|
|
}
|
|
if (!smart) {
|
|
printf("ARRIVED simple\n");
|
|
agent_walk_stop(obj);
|
|
where = obj->get_meta("_target");
|
|
emit_signal("arrived", obj, where);
|
|
printf("arrived to %ls\n", String(where).c_str());
|
|
} else {
|
|
printf("ARRIVED smart\n");
|
|
smm->arrived(sm, sp);
|
|
}
|
|
} else {
|
|
if (obj->has_meta("_target_node")) {
|
|
Node *tn = obj->get_meta("_target_node");
|
|
if (tn) {
|
|
PhysicsBody *pbt = Object::cast_to<PhysicsBody>(tn);
|
|
if (pbt) {
|
|
Vector3 target_cur = pbt->get_global_transform().origin;
|
|
obj->set_meta("_target", target_cur);
|
|
} else {
|
|
Spatial *spt = Object::cast_to<Spatial>(tn);
|
|
if (spt) {
|
|
Vector3 target_cur = spt->get_global_transform().origin;
|
|
obj->set_meta("_target", target_cur);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
void Characters_::agent_walk_stop(Object *obj)
|
|
{
|
|
if (obj->has_meta("_target"))
|
|
obj->remove_meta("_target");
|
|
if (obj->has_meta("_target_node"))
|
|
obj->remove_meta("_target_node");
|
|
}
|
|
void Characters_::walkto_agent_node(Node *ch, const Node *target)
|
|
{
|
|
if (!ch->has_meta("agent_id"))
|
|
return;
|
|
ch->set_meta("_target_node", target->get_instance_id());
|
|
const Spatial *sp = Object::cast_to<Spatial>(target);
|
|
if (sp) {
|
|
Vector3 tpos = sp->get_global_transform().origin;
|
|
walkto_agent(ch, tpos);
|
|
}
|
|
}
|
|
void Characters_::walkto_agent(Node *ch, const Vector3 &target)
|
|
{
|
|
assert(crowd);
|
|
if (ch->has_meta("_target")) {
|
|
Vector3 otarget = ch->get_meta("_target");
|
|
if (otarget == target)
|
|
return;
|
|
}
|
|
assert(ch->has_meta("agent_id"));
|
|
int agent_id = ch->get_meta("agent_id");
|
|
ch->set_meta("_target", target);
|
|
crowd->set_agent_target_position(agent_id, target);
|
|
printf("target set to %ls\n", String(target).c_str());
|
|
}
|
|
void Characters_::_notification(int p_what)
|
|
{
|
|
List<Node *> char_node_list;
|
|
List<Node *> frozen_node_list;
|
|
List<Node *>::Element *e;
|
|
switch(p_what) {
|
|
case NOTIFICATION_PROCESS:
|
|
if (!initialized)
|
|
return;
|
|
get_tree()->get_nodes_in_group("character", &char_node_list);
|
|
get_tree()->get_nodes_in_group("frozen", &frozen_node_list);
|
|
if (debug) {
|
|
debug->clear();
|
|
debug->begin(Mesh::PRIMITIVE_LINES);
|
|
debug->set_color(Color(1, 0, 0, 1));
|
|
}
|
|
for (e = char_node_list.front(); e; e = e->next()) {
|
|
debug->set_color(Color(1, 0, 0, 1));
|
|
Node *ch = e->get();
|
|
if (!ch->has_meta("animation_tree"))
|
|
continue;
|
|
process_character(ch, false);
|
|
if (debug) {
|
|
Spatial *sp = Object::cast_to<Spatial>(ch);
|
|
debug->set_color(Color(1, 0.5f, 1, 1));
|
|
Transform xf = sp->get_global_transform();
|
|
Vector3 p1 = xf.origin;
|
|
// Vector3 b = -xf.basis[2];
|
|
// b.x = -b.x;
|
|
xf.origin = Vector3();
|
|
Vector3 b = xf.xform(Vector3(0.0f, 0.0f, -1.0f));
|
|
Vector3 p2 = p1 + b * 2.0f;
|
|
debug->add_vertex(p1);
|
|
debug->add_vertex(p2);
|
|
if (ch->has_meta("agent_velocity")) {
|
|
Vector3 p3 = p1 + ch->get_meta("agent_velocity");
|
|
debug->set_color(Color(0.5f, 1, 0.5f, 1));
|
|
debug->add_vertex(p1);
|
|
debug->add_vertex(p3);
|
|
}
|
|
}
|
|
}
|
|
if (debug)
|
|
debug->set_color(Color(1, 0, 0, 1));
|
|
for (e = frozen_node_list.front(); e; e = e->next()) {
|
|
Node *ch = e->get();
|
|
if (!ch->has_meta("animation_node"))
|
|
continue;
|
|
process_character(ch, true);
|
|
}
|
|
if (debug)
|
|
debug->end();
|
|
break;
|
|
case NOTIFICATION_PHYSICS_PROCESS:
|
|
if (!initialized)
|
|
return;
|
|
get_tree()->get_nodes_in_group("character", &char_node_list);
|
|
#if 0
|
|
if (debug) {
|
|
debug->clear();
|
|
debug->begin(Mesh::PRIMITIVE_LINES);
|
|
debug->set_color(Color(1, 0, 0, 1));
|
|
}
|
|
#endif
|
|
for (e = char_node_list.front(); e; e = e->next()) {
|
|
Node *ch = e->get();
|
|
if (!ch->has_meta("animation_tree"))
|
|
continue;
|
|
if (!ch->has_meta("orientation"))
|
|
continue;
|
|
character_physics(ch);
|
|
Spatial *sp = Object::cast_to<Spatial>(ch);
|
|
Vector3 direction = -sp->get_global_transform().basis[2];
|
|
ch->set_meta("direction", direction);
|
|
}
|
|
#if 0
|
|
if (debug)
|
|
debug->end();
|
|
#endif
|
|
break;
|
|
case NOTIFICATION_READY:
|
|
arrive_precision = GLOBAL_DEF("ai/locomotion/arrive_precision", 0.2f);
|
|
set_process(true);
|
|
set_physics_process(true);
|
|
smm = memnew(SmartObjectManager);
|
|
add_child(smm);
|
|
break;
|
|
}
|
|
}
|
|
void Characters_::_bind_methods()
|
|
{
|
|
ClassDB::bind_method(D_METHOD("walkto", "ch", "target"), &Characters_::walkto);
|
|
ClassDB::bind_method(D_METHOD("walkto_node", "ch", "target"), &Characters_::walkto_node);
|
|
ClassDB::bind_method(D_METHOD("set_navmesh", "mesh", "xform"), &Characters_::set_navmesh);
|
|
ClassDB::bind_method(D_METHOD("forward_probe", "body", "lookahead", "start_height", "end_height", "height_step"), &Characters_::forward_probe);
|
|
ClassDB::bind_method(D_METHOD("get_animation_node", "npc"), &Characters_::get_animation_node);
|
|
ClassDB::bind_method(D_METHOD("handle_cmdq", "npc"), &Characters_::handle_cmdq);
|
|
ClassDB::bind_method(D_METHOD("animation_node_travel", "npc", "anim"), &Characters_::animation_node_travel);
|
|
ClassDB::bind_method(D_METHOD("set_walk_speed", "npc", "speed", "strafe"), &Characters_::set_walk_speed);
|
|
ClassDB::bind_method(D_METHOD("get_walk_speed", "npc"), &Characters_::get_walk_speed);
|
|
ClassDB::bind_method(D_METHOD("set_crowd", "crowd"), &Characters_::set_crowd_);
|
|
ClassDB::bind_method(D_METHOD("get_crowd"), &Characters_::get_crowd);
|
|
ClassDB::bind_method(D_METHOD("walkto_agent", "ch", "target"), &Characters_::walkto_agent);
|
|
ClassDB::bind_method(D_METHOD("walkto_agent_node", "ch", "target"), &Characters_::walkto_agent_node);
|
|
ClassDB::bind_method(D_METHOD("character_physics", "obj"), &Characters_::character_physics);
|
|
ADD_SIGNAL(MethodInfo("arrived", PropertyInfo(Variant::OBJECT, "obj"), PropertyInfo(Variant::VECTOR3, "where")));
|
|
}
|
|
|
|
void Characters_::process_frozen_character(Node *npc, const Vector3 &tposition)
|
|
{
|
|
float delta = npc->get_process_delta_time();
|
|
Vector3 tgt = tposition;
|
|
Spatial *sp = Object::cast_to<Spatial>(npc);
|
|
assert(sp);
|
|
Vector3 position = sp->get_global_transform().origin;
|
|
float dst = position.distance_squared_to(tgt);
|
|
tgt.y = position.y;
|
|
float tconst = delta / dst;
|
|
Transform xform = sp->get_global_transform();
|
|
xform = xform.looking_at(tgt, Vector3(0, 1, 0));
|
|
xform.origin = position.linear_interpolate(tgt, tconst);
|
|
sp->set_global_transform(xform);
|
|
}
|
|
|
|
void Characters_::process_normal_character(Node *npc, const Vector3 &tposition)
|
|
{
|
|
#if 0
|
|
Spatial *sp = Object::cast_to<Spatial>(npc);
|
|
assert(sp);
|
|
float delta = npc->get_process_delta_time();
|
|
Vector3 tgt = tposition;
|
|
Vector3 position = sp->get_global_transform().origin;
|
|
tgt.y = position.y;
|
|
#endif
|
|
}
|
|
|
|
void Characters_::process_character(Node *node, bool frozen)
|
|
{
|
|
int id = node->get_instance_id();
|
|
Spatial *sp = Object::cast_to<Spatial>(node);
|
|
if (!sp)
|
|
return;
|
|
Vector3 target;
|
|
Vector3 position = sp->get_global_transform().origin;
|
|
if (static_targets.has(id))
|
|
target = static_targets[id];
|
|
else if (dynamic_targets.has(id)) {
|
|
int target_id = dynamic_targets[id];
|
|
Object * obj = ObjectDB::get_instance(target_id);
|
|
if (!obj) {
|
|
dynamic_targets.erase(id);
|
|
return;
|
|
}
|
|
Spatial *target_node = Object::cast_to<Spatial>(obj);
|
|
if (!target_node) {
|
|
dynamic_targets.erase(id);
|
|
return;
|
|
}
|
|
target = target_node->get_global_transform().origin;
|
|
} else /* no target - no problem */
|
|
return;
|
|
float distance = position.distance_squared_to(target);
|
|
int i;
|
|
if (!paths.has(id)) {
|
|
Vector<Vector3> points;
|
|
Vector<int> flags;
|
|
Vector3 start = query->nearest_point(position, Vector3(1, 1, 1), filter);
|
|
Vector3 end = query->nearest_point(target, Vector3(1, 1, 1), filter);
|
|
query->find_path_array(start, end, Vector3(1, 1, 1), filter, points, flags);
|
|
assert(points.size() > 0);
|
|
paths[id] = points;
|
|
}
|
|
if (debug)
|
|
for (i = 0; i < paths[id].size() - 1; i++) {
|
|
debug->add_vertex(paths[id][i]);
|
|
debug->add_vertex(paths[id][i + 1]);
|
|
}
|
|
if (distance < 0.5) {
|
|
if (static_targets.has(id))
|
|
static_targets.erase(id);
|
|
if (dynamic_targets.has(id))
|
|
dynamic_targets.erase(id);
|
|
if (paths.has(id))
|
|
paths.erase(id);
|
|
emit_signal("arrived", node);
|
|
} else if (frozen) {
|
|
Vector3 tgt = target;
|
|
if (paths[id].size() > 0)
|
|
tgt = paths[id][0];
|
|
float dst = position.distance_squared_to(tgt);
|
|
while (dst < 0.0001f && paths[id].size() > 0) {
|
|
paths[id].erase(tgt);
|
|
tgt = paths[id][0];
|
|
dst = position.distance_squared_to(tgt);
|
|
}
|
|
process_frozen_character(node, tgt);
|
|
|
|
} else {
|
|
Vector3 tgt = target;
|
|
if (paths[id].size() > 0)
|
|
tgt = paths[id][0];
|
|
float dst = position.distance_squared_to(tgt);
|
|
while (dst < 0.0001f && paths[id].size() > 0) {
|
|
paths[id].erase(tgt);
|
|
tgt = paths[id][0];
|
|
dst = position.distance_squared_to(tgt);
|
|
}
|
|
process_normal_character(node, tgt);
|
|
}
|
|
}
|
|
|
|
Characters_::~Characters_()
|
|
{
|
|
memdelete(query);
|
|
}
|
|
|
|
void Characters_::walkto(const Node *ch, const Vector3 &target)
|
|
{
|
|
static_targets[ch->get_instance_id()] = target;
|
|
}
|
|
|
|
void Characters_::walkto_node(const Node *ch, const Node *target)
|
|
{
|
|
dynamic_targets[ch->get_instance_id()] = target->get_instance_id();
|
|
}
|
|
|
|
void Characters_::set_navmesh(Ref<DetourNavigationMesh> mesh, const Transform &xform)
|
|
{
|
|
query->init(mesh, xform);
|
|
filter.instance();
|
|
debug = memnew(ImmediateGeometry);
|
|
add_child(debug);
|
|
initialized = true;
|
|
Ref<SpatialMaterial> mat;
|
|
mat.instance();
|
|
mat->set_flag(SpatialMaterial::FLAG_UNSHADED, true);
|
|
mat->set_flag(SpatialMaterial::FLAG_DISABLE_DEPTH_TEST, true);
|
|
mat->set_flag(SpatialMaterial::FLAG_ALBEDO_FROM_VERTEX_COLOR, true);
|
|
debug->set_material_override(mat);
|
|
}
|
|
float Characters_::forward_probe(Node *body, float lookahead,
|
|
float start_height, float end_height,
|
|
float height_step)
|
|
{
|
|
float result = 0.0f, d;
|
|
Ref<World> world;
|
|
PhysicsDirectSpaceState *space;
|
|
PhysicsDirectSpaceState::RayResult rr;
|
|
Set<RID> exclude;
|
|
PhysicsBody *pbody = Object::cast_to<PhysicsBody>(body);
|
|
assert(pbody);
|
|
|
|
world = pbody->get_world();
|
|
if (!world.ptr())
|
|
return result;
|
|
space = PhysicsServer::get_singleton()->space_get_direct_state(world->get_space());
|
|
d = start_height;
|
|
exclude.insert(pbody->get_rid());
|
|
while (d < end_height) {
|
|
Transform g = pbody->get_global_transform();
|
|
Vector3 o = g.origin + Vector3(0, 1, 0) * d;
|
|
Vector3 f = g.xform(Vector3(0, 0, -1) * lookahead + Vector3(0, 1, 0) * d);
|
|
if (space->intersect_ray(o, f, rr, exclude)) {
|
|
int against = rr.collider_id;
|
|
Object * obj = ObjectDB::get_instance(against);
|
|
if (obj) {
|
|
Node *col = Object::cast_to<Node>(obj);
|
|
if (col && !col->is_in_group("character"))
|
|
result = d;
|
|
}
|
|
}
|
|
d += height_step;
|
|
}
|
|
return result;
|
|
}
|
|
|
|
void Characters_::set_crowd(DetourCrowdManager *crowd)
|
|
{
|
|
this->crowd = crowd;
|
|
}
|
|
void Characters_::set_crowd_(Node *crowd_node)
|
|
{
|
|
DetourCrowdManager *crowd =
|
|
Object::cast_to<DetourCrowdManager>(crowd_node);
|
|
if (crowd)
|
|
set_crowd(crowd);
|
|
}
|
|
|
|
DetourCrowdManager *Characters_::get_crowd() const
|
|
{
|
|
return crowd;
|
|
}
|
|
|