diff --git a/OdeWorld.cc b/OdeWorld.cc index e20897e..149d210 100644 --- a/OdeWorld.cc +++ b/OdeWorld.cc @@ -204,8 +204,18 @@ dGeomID OdeWorld::addPlane(const string & name, bool static_data, { if (args.size() != 6) return 0; - float a, b, c, d; - /* TODO: find a, b, c, d from args */ + + dMatrix3 r; + dRFromEulerAngles(r, args[3], args[4], args[5]); + dVector3 default_plane_direction = {0, 0, 1, 0}; + dVector3 rotated_plane_direction; + dMultiply0(rotated_plane_direction, default_plane_direction, r, 1, 3, 3); + + float a = rotated_plane_direction[0]; + float b = rotated_plane_direction[1]; + float c = rotated_plane_direction[2]; + float d = a * args[0] + b * args[1] + c * args[2]; + dGeomID id = dCreatePlane(m_space, a, b, c, d); return id; }