451 lines
13 KiB
C++
451 lines
13 KiB
C++
/*************************************************************************/
|
|
/* test_misc.cpp */
|
|
/*************************************************************************/
|
|
/* This file is part of: */
|
|
/* GODOT ENGINE */
|
|
/* https://godotengine.org */
|
|
/*************************************************************************/
|
|
/* Copyright (c) 2007-2016 Juan Linietsky, Ariel Manzur. */
|
|
/* */
|
|
/* Permission is hereby granted, free of charge, to any person obtaining */
|
|
/* a copy of this software and associated documentation files (the */
|
|
/* "Software"), to deal in the Software without restriction, including */
|
|
/* without limitation the rights to use, copy, modify, merge, publish, */
|
|
/* distribute, sublicense, and/or sell copies of the Software, and to */
|
|
/* permit persons to whom the Software is furnished to do so, subject to */
|
|
/* the following conditions: */
|
|
/* */
|
|
/* The above copyright notice and this permission notice shall be */
|
|
/* included in all copies or substantial portions of the Software. */
|
|
/* */
|
|
/* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, */
|
|
/* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF */
|
|
/* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.*/
|
|
/* IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY */
|
|
/* CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, */
|
|
/* TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE */
|
|
/* SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. */
|
|
/*************************************************************************/
|
|
#include "test_misc.h"
|
|
#include "math_funcs.h"
|
|
#include "os/main_loop.h"
|
|
#include "print_string.h"
|
|
#include "servers/visual_server.h"
|
|
|
|
namespace TestMisc {
|
|
|
|
struct ConvexTestResult {
|
|
|
|
Vector3 edgeA[2];
|
|
Vector3 edgeB[2];
|
|
bool valid;
|
|
Vector3 contactA;
|
|
Vector3 contactB;
|
|
Vector3 contactNormal;
|
|
float depth;
|
|
|
|
/*
|
|
Vector3 contactA;
|
|
Vector3 contactB;
|
|
Vector3 contactNormal;
|
|
Vector3 contactX;
|
|
Vector3 contactY;
|
|
Vector3 edgeA[2];
|
|
Vector3 edgeB[2];
|
|
float depth;
|
|
bool valid;
|
|
bool isEdgeEdge;
|
|
bool needTransform;
|
|
neBool ComputerEdgeContactPoint(ConvexTestResult & res);
|
|
neBool ComputerEdgeContactPoint2(float & au, float & bu);
|
|
void Reverse()
|
|
{
|
|
neSwap(contactA, contactB);
|
|
contactNormal *= -1.0f;
|
|
}*/
|
|
bool ComputerEdgeContactPoint2(float &au, float &bu);
|
|
};
|
|
|
|
bool ConvexTestResult::ComputerEdgeContactPoint2(float &au, float &bu) {
|
|
float d1343, d4321, d1321, d4343, d2121;
|
|
float numer, denom;
|
|
|
|
Vector3 p13;
|
|
Vector3 p43;
|
|
Vector3 p21;
|
|
Vector3 diff;
|
|
|
|
p13 = (edgeA[0]) - (edgeB[0]);
|
|
p43 = (edgeB[1]) - (edgeB[0]);
|
|
|
|
if (p43.length_squared() < CMP_EPSILON2) {
|
|
valid = false;
|
|
goto ComputerEdgeContactPoint2_Exit;
|
|
}
|
|
|
|
p21 = (edgeA[1]) - (edgeA[0]);
|
|
|
|
if (p21.length_squared() < CMP_EPSILON2) {
|
|
valid = false;
|
|
goto ComputerEdgeContactPoint2_Exit;
|
|
}
|
|
|
|
d1343 = p13.dot(p43);
|
|
d4321 = p43.dot(p21);
|
|
d1321 = p13.dot(p21);
|
|
d4343 = p43.dot(p43);
|
|
d2121 = p21.dot(p21);
|
|
|
|
denom = d2121 * d4343 - d4321 * d4321;
|
|
|
|
if (ABS(denom) < CMP_EPSILON) {
|
|
valid = false;
|
|
|
|
goto ComputerEdgeContactPoint2_Exit;
|
|
}
|
|
|
|
numer = d1343 * d4321 - d1321 * d4343;
|
|
au = numer / denom;
|
|
bu = (d1343 + d4321 * (au)) / d4343;
|
|
|
|
if (au < 0.0f || au >= 1.0f) {
|
|
valid = false;
|
|
} else if (bu < 0.0f || bu >= 1.0f) {
|
|
valid = false;
|
|
} else {
|
|
valid = true;
|
|
}
|
|
{
|
|
Vector3 tmpv;
|
|
|
|
tmpv = p21 * au;
|
|
contactA = (edgeA[0]) + tmpv;
|
|
|
|
tmpv = p43 * bu;
|
|
contactB = (edgeB[0]) + tmpv;
|
|
}
|
|
|
|
diff = contactA - contactB;
|
|
|
|
depth = Math::sqrt(diff.dot(diff));
|
|
|
|
return true;
|
|
|
|
ComputerEdgeContactPoint2_Exit:
|
|
|
|
return false;
|
|
}
|
|
|
|
struct neCollisionResult {
|
|
|
|
float depth;
|
|
bool penetrate;
|
|
Matrix3 collisionFrame;
|
|
Vector3 contactA;
|
|
Vector3 contactB;
|
|
};
|
|
|
|
struct TConvex {
|
|
|
|
float radius;
|
|
float half_height;
|
|
float CylinderRadius() const { return radius; }
|
|
float CylinderHalfHeight() const { return half_height; }
|
|
};
|
|
|
|
float GetDistanceFromLine2(Vector3 v, Vector3 &project, const Vector3 &pointA, const Vector3 &pointB) {
|
|
Vector3 ba = pointB - pointA;
|
|
|
|
float len = ba.length();
|
|
|
|
if (len < CMP_EPSILON)
|
|
ba = Vector3();
|
|
else
|
|
ba *= 1.0f / len;
|
|
|
|
Vector3 pa = v - pointA;
|
|
|
|
float k = pa.dot(ba);
|
|
|
|
project = pointA + ba * k;
|
|
|
|
Vector3 diff = v - project;
|
|
|
|
return diff.length();
|
|
}
|
|
|
|
void TestCylinderVertEdge(neCollisionResult &result, Vector3 &edgeA1, Vector3 &edgeA2, Vector3 &vertB,
|
|
TConvex &cA, TConvex &cB, Transform &transA, Transform &transB, bool flip) {
|
|
Vector3 project;
|
|
|
|
float dist = GetDistanceFromLine2(vertB, project, edgeA1, edgeA2);
|
|
|
|
float depth = cA.CylinderRadius() + cB.CylinderRadius() - dist;
|
|
|
|
if (depth <= 0.0f)
|
|
return;
|
|
|
|
if (depth <= result.depth)
|
|
return;
|
|
|
|
result.penetrate = true;
|
|
|
|
result.depth = depth;
|
|
|
|
if (!flip) {
|
|
result.collisionFrame.set_axis(2, (project - vertB).normalized());
|
|
|
|
result.contactA = project - result.collisionFrame.get_axis(2) * cA.CylinderRadius();
|
|
|
|
result.contactB = vertB + result.collisionFrame.get_axis(2) * cB.CylinderRadius();
|
|
} else {
|
|
|
|
result.collisionFrame.set_axis(2, (vertB - project).normalized());
|
|
|
|
result.contactA = vertB - result.collisionFrame.get_axis(2) * cB.CylinderRadius();
|
|
|
|
result.contactB = project + result.collisionFrame.get_axis(2) * cA.CylinderRadius();
|
|
}
|
|
}
|
|
|
|
void TestCylinderVertVert(neCollisionResult &result, Vector3 &vertA, Vector3 &vertB,
|
|
TConvex &cA, TConvex &cB, Transform &transA, Transform &transB) {
|
|
Vector3 diff = vertA - vertB;
|
|
|
|
float dist = diff.length();
|
|
|
|
float depth = cA.CylinderRadius() + cB.CylinderRadius() - dist;
|
|
|
|
if (depth <= 0.0f)
|
|
return;
|
|
|
|
if (depth <= result.depth)
|
|
return;
|
|
|
|
result.penetrate = true;
|
|
|
|
result.depth = depth;
|
|
|
|
result.collisionFrame.set_axis(2, diff * (1.0f / dist));
|
|
|
|
result.contactA = vertA - result.collisionFrame.get_axis(2) * cA.CylinderRadius();
|
|
|
|
result.contactB = vertB + result.collisionFrame.get_axis(2) * cB.CylinderRadius();
|
|
}
|
|
|
|
void Cylinder2CylinderTest(neCollisionResult &result, TConvex &cA, Transform &transA, TConvex &cB, Transform &transB) {
|
|
result.penetrate = false;
|
|
|
|
Vector3 dir = transA.basis.get_axis(1).cross(transB.basis.get_axis(1));
|
|
|
|
float len = dir.length();
|
|
|
|
// bool isParallel = len<CMP_EPSILON;
|
|
|
|
// int doVertCheck = 0;
|
|
|
|
ConvexTestResult cr;
|
|
|
|
cr.edgeA[0] = transA.origin + transA.basis.get_axis(1) * cA.CylinderHalfHeight();
|
|
cr.edgeA[1] = transA.origin - transA.basis.get_axis(1) * cA.CylinderHalfHeight();
|
|
cr.edgeB[0] = transB.origin + transB.basis.get_axis(1) * cB.CylinderHalfHeight();
|
|
cr.edgeB[1] = transB.origin - transB.basis.get_axis(1) * cB.CylinderHalfHeight();
|
|
|
|
// float dot = transA.basis.get_axis(1).dot(transB.basis.get_axis(1));
|
|
|
|
if (len > CMP_EPSILON) {
|
|
float au, bu;
|
|
|
|
cr.ComputerEdgeContactPoint2(au, bu);
|
|
|
|
if (cr.valid) {
|
|
float depth = cA.CylinderRadius() + cB.CylinderRadius() - cr.depth;
|
|
|
|
if (depth <= 0.0f)
|
|
return;
|
|
|
|
result.depth = depth;
|
|
|
|
result.penetrate = true;
|
|
|
|
result.collisionFrame.set_axis(2, (cr.contactA - cr.contactB) * (1.0f / cr.depth));
|
|
|
|
result.contactA = cr.contactA - result.collisionFrame.get_axis(2) * cA.CylinderRadius();
|
|
|
|
result.contactB = cr.contactB + result.collisionFrame.get_axis(2) * cB.CylinderRadius();
|
|
|
|
return;
|
|
}
|
|
}
|
|
result.depth = -1.0e6f;
|
|
|
|
int i;
|
|
|
|
for (i = 0; i < 2; i++) {
|
|
//project onto edge b
|
|
|
|
Vector3 diff = cr.edgeA[i] - cr.edgeB[1];
|
|
|
|
float dot = diff.dot(transB.basis.get_axis(1));
|
|
|
|
if (dot < 0.0f) {
|
|
TestCylinderVertVert(result, cr.edgeA[i], cr.edgeB[1], cA, cB, transA, transB);
|
|
} else if (dot > (2.0f * cB.CylinderHalfHeight())) {
|
|
TestCylinderVertVert(result, cr.edgeA[i], cr.edgeB[0], cA, cB, transA, transB);
|
|
} else {
|
|
TestCylinderVertEdge(result, cr.edgeB[0], cr.edgeB[1], cr.edgeA[i], cB, cA, transB, transA, true);
|
|
}
|
|
}
|
|
for (i = 0; i < 2; i++) {
|
|
//project onto edge b
|
|
|
|
Vector3 diff = cr.edgeB[i] - cr.edgeA[1];
|
|
|
|
float dot = diff.dot(transA.basis.get_axis(1));
|
|
|
|
if (dot < 0.0f) {
|
|
TestCylinderVertVert(result, cr.edgeB[i], cr.edgeA[1], cA, cB, transA, transB);
|
|
} else if (dot > (2.0f * cB.CylinderHalfHeight())) {
|
|
TestCylinderVertVert(result, cr.edgeB[i], cr.edgeA[0], cA, cB, transA, transB);
|
|
} else {
|
|
TestCylinderVertEdge(result, cr.edgeA[0], cr.edgeA[1], cr.edgeB[i], cA, cB, transA, transB, false);
|
|
}
|
|
}
|
|
}
|
|
|
|
class TestMainLoop : public MainLoop {
|
|
|
|
RID meshA;
|
|
RID meshB;
|
|
RID poly;
|
|
RID instance;
|
|
RID camera;
|
|
RID viewport;
|
|
RID boxA;
|
|
RID boxB;
|
|
RID scenario;
|
|
|
|
Transform rot_a;
|
|
Transform rot_b;
|
|
|
|
bool quit;
|
|
|
|
public:
|
|
virtual void input_event(const InputEvent &p_event) {
|
|
|
|
if (p_event.type == InputEvent::MOUSE_MOTION && p_event.mouse_motion.button_mask & BUTTON_MASK_LEFT) {
|
|
|
|
rot_b.origin.y += -p_event.mouse_motion.relative_y / 100.0;
|
|
rot_b.origin.x += p_event.mouse_motion.relative_x / 100.0;
|
|
}
|
|
if (p_event.type == InputEvent::MOUSE_MOTION && p_event.mouse_motion.button_mask & BUTTON_MASK_MIDDLE) {
|
|
|
|
//rot_b.origin.x+=-p_event.mouse_motion.relative_y/100.0;
|
|
rot_b.origin.z += p_event.mouse_motion.relative_x / 100.0;
|
|
}
|
|
if (p_event.type == InputEvent::MOUSE_MOTION && p_event.mouse_motion.button_mask & BUTTON_MASK_RIGHT) {
|
|
|
|
float rot_x = -p_event.mouse_motion.relative_y / 100.0;
|
|
float rot_y = p_event.mouse_motion.relative_x / 100.0;
|
|
rot_b.basis = rot_b.basis * Matrix3(Vector3(1, 0, 0), rot_x) * Matrix3(Vector3(0, 1, 0), rot_y);
|
|
}
|
|
}
|
|
virtual void request_quit() {
|
|
|
|
quit = true;
|
|
}
|
|
virtual void init() {
|
|
|
|
VisualServer *vs = VisualServer::get_singleton();
|
|
|
|
camera = vs->camera_create();
|
|
|
|
viewport = vs->viewport_create();
|
|
vs->viewport_attach_to_screen(viewport);
|
|
vs->viewport_attach_camera(viewport, camera);
|
|
vs->camera_set_transform(camera, Transform(Matrix3(), Vector3(0, 0, 3)));
|
|
|
|
/* CONVEX SHAPE */
|
|
|
|
DVector<Plane> cylinder_planes = Geometry::build_cylinder_planes(0.5, 2, 9, Vector3::AXIS_Y);
|
|
RID cylinder_material = vs->fixed_material_create();
|
|
vs->fixed_material_set_param(cylinder_material, VisualServer::FIXED_MATERIAL_PARAM_DIFFUSE, Color(0.8, 0.2, 0.9));
|
|
vs->material_set_flag(cylinder_material, VisualServer::MATERIAL_FLAG_ONTOP, true);
|
|
//vs->material_set_flag( cylinder_material, VisualServer::MATERIAL_FLAG_WIREFRAME,true);
|
|
vs->material_set_flag(cylinder_material, VisualServer::MATERIAL_FLAG_DOUBLE_SIDED, true);
|
|
vs->material_set_flag(cylinder_material, VisualServer::MATERIAL_FLAG_UNSHADED, true);
|
|
|
|
RID cylinder_mesh = vs->mesh_create();
|
|
Geometry::MeshData cylinder_data = Geometry::build_convex_mesh(cylinder_planes);
|
|
vs->mesh_add_surface_from_mesh_data(cylinder_mesh, cylinder_data);
|
|
vs->mesh_surface_set_material(cylinder_mesh, 0, cylinder_material);
|
|
|
|
meshA = vs->instance_create2(cylinder_mesh, scenario);
|
|
meshB = vs->instance_create2(cylinder_mesh, scenario);
|
|
boxA = vs->instance_create2(vs->get_test_cube(), scenario);
|
|
boxB = vs->instance_create2(vs->get_test_cube(), scenario);
|
|
|
|
/*
|
|
RID lightaux = vs->light_create( VisualServer::LIGHT_OMNI );
|
|
vs->light_set_var( lightaux, VisualServer::LIGHT_VAR_RADIUS, 80 );
|
|
vs->light_set_var( lightaux, VisualServer::LIGHT_VAR_ATTENUATION, 1 );
|
|
vs->light_set_var( lightaux, VisualServer::LIGHT_VAR_ENERGY, 1.5 );
|
|
light = vs->instance_create2( lightaux );
|
|
*/
|
|
RID lightaux = vs->light_create(VisualServer::LIGHT_DIRECTIONAL);
|
|
//vs->light_set_color( lightaux, VisualServer::LIGHT_COLOR_AMBIENT, Color(0.0,0.0,0.0) );
|
|
//vs->light_set_shadow( lightaux, true );
|
|
RID light = vs->instance_create2(lightaux, scenario);
|
|
|
|
//rot_a=Transform(Matrix3(Vector3(1,0,0),Math_PI/2.0),Vector3());
|
|
rot_b = Transform(Matrix3(), Vector3(2, 0, 0));
|
|
|
|
//rot_x=0;
|
|
//rot_y=0;
|
|
quit = false;
|
|
}
|
|
virtual bool idle(float p_time) {
|
|
|
|
VisualServer *vs = VisualServer::get_singleton();
|
|
|
|
vs->instance_set_transform(meshA, rot_a);
|
|
vs->instance_set_transform(meshB, rot_b);
|
|
|
|
neCollisionResult res;
|
|
TConvex a;
|
|
a.radius = 0.5;
|
|
a.half_height = 1;
|
|
Cylinder2CylinderTest(res, a, rot_a, a, rot_b);
|
|
if (res.penetrate) {
|
|
|
|
Matrix3 scale;
|
|
scale.scale(Vector3(0.1, 0.1, 0.1));
|
|
vs->instance_set_transform(boxA, Transform(scale, res.contactA));
|
|
vs->instance_set_transform(boxB, Transform(scale, res.contactB));
|
|
print_line("depth: " + rtos(res.depth));
|
|
} else {
|
|
|
|
Matrix3 scale;
|
|
scale.scale(Vector3());
|
|
vs->instance_set_transform(boxA, Transform(scale, res.contactA));
|
|
vs->instance_set_transform(boxB, Transform(scale, res.contactB));
|
|
}
|
|
print_line("collided: " + itos(res.penetrate));
|
|
|
|
return false;
|
|
}
|
|
|
|
virtual bool iteration(float p_time) {
|
|
|
|
return quit;
|
|
}
|
|
virtual void finish() {
|
|
}
|
|
};
|
|
|
|
MainLoop *test() {
|
|
|
|
return memnew(TestMainLoop);
|
|
}
|
|
}
|