/*************************************************************************/ /* test_misc.cpp */ /*************************************************************************/ /* This file is part of: */ /* GODOT ENGINE */ /* http://www.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 "servers/visual_server.h" #include "os/main_loop.h" #include "math_funcs.h" #include "print_string.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 ); } }