From a5232c2b045185703c3285fed59db153942d68db Mon Sep 17 00:00:00 2001 From: David Rose Date: Tue, 19 Dec 2000 19:06:22 +0000 Subject: [PATCH] *** empty log message *** --- panda/src/collide/collisionHandlerFloor.cxx | 21 +- panda/src/linmath/compose_matrix.cxx | 314 +++++++++++++++++++- panda/src/linmath/compose_matrix.h | 26 ++ panda/src/net/connectionReader.cxx | 11 +- panda/src/net/datagramUDPHeader.cxx | 25 +- panda/src/net/datagramUDPHeader.h | 4 +- panda/src/tform/driveInterface.cxx | 71 ++++- panda/src/tform/driveInterface.h | 6 + 8 files changed, 432 insertions(+), 46 deletions(-) diff --git a/panda/src/collide/collisionHandlerFloor.cxx b/panda/src/collide/collisionHandlerFloor.cxx index f08a8cf55e..dfa912fd29 100644 --- a/panda/src/collide/collisionHandlerFloor.cxx +++ b/panda/src/collide/collisionHandlerFloor.cxx @@ -86,14 +86,23 @@ handle_entries() { } // Now set our height accordingly. - if (collide_cat.is_debug()) { - collide_cat.debug() - << "Resetting height to " << max_height << " + " << _offset << "\n"; - } LMatrix4f mat; def.get_mat(mat); - mat(3, 2) = max_height + _offset; - def.set_mat(mat); + if (!IS_THRESHOLD_EQUAL(mat(3, 2), max_height + _offset, 0.001)) { + if (collide_cat.is_debug()) { + collide_cat.debug() + << "Resetting height to " << max_height << " + " + << _offset << "\n"; + } + mat(3, 2) = max_height + _offset; + def.set_mat(mat); + } else { + if (collide_cat.is_spam()) { + collide_cat.spam() + << "Height is already at " << max_height << " + " + << _offset << "\n"; + } + } } } } diff --git a/panda/src/linmath/compose_matrix.cxx b/panda/src/linmath/compose_matrix.cxx index ae62bfa4af..d9ac9a616b 100644 --- a/panda/src/linmath/compose_matrix.cxx +++ b/panda/src/linmath/compose_matrix.cxx @@ -125,6 +125,81 @@ unwind_yup_rotation(LMatrix3 &mat, LVecBase3 &hpr) { hpr[2] = roll; } +//////////////////////////////////////////////////////////////////// +// Function: unwind_yup_rotation +// Description: Extracts the rotation about the x, y, and z axes from +// the given hpr & scale matrix, given the indicated +// roll amount as a hint. Adjusts the matrix to +// eliminate the rotation. +// +// This function assumes the matrix is stored in a +// right-handed Y-up coordinate system. +//////////////////////////////////////////////////////////////////// +template +static void +unwind_yup_rotation(LMatrix3 &mat, LVecBase3 &hpr, + NumType roll) { + typedef LMatrix3 Matrix; + + // Extract the axes from the matrix. + LVector3 x, y, z; + x = mat.get_row(0); + y = mat.get_row(1); + z = mat.get_row(2); + + // Unwind the roll from the axes, and continue. + Matrix rot_z; + rot_z = Matrix::rotate_mat(-roll, LVector3(0.0, 0.0, 1.0), + CS_yup_right); + + x = x * rot_z; + y = y * rot_z; + z = z * rot_z; + + // Project the rotated X into the XZ plane. + LVector2 xz(x[0], x[2]); + xz = normalize(xz); + + // Compute the rotation about the +Y (up) axis. This is yaw, or + // "heading". + NumType heading = rad_2_deg(-atan2(xz[1], xz[0])); + + // Unwind the heading, and continue. + Matrix rot_y; + rot_y = Matrix::rotate_mat(-heading, LVector3(0.0, 1.0, 0.0), + CS_yup_right); + + x = x * rot_y; + y = y * rot_y; + z = z * rot_y; + + // Project the rotated Z into the YZ plane. + LVector2 yz(z[1], z[2]); + yz = normalize(yz); + + // Compute the rotation about the +X (right) axis. This is pitch. + NumType pitch = rad_2_deg(-atan2(yz[0], yz[1])); + + // Unwind the pitch. + Matrix rot_x; + rot_x = Matrix::rotate_mat(-pitch, LVector3(1.0, 0.0, 0.0), + CS_yup_right); + + x = x * rot_x; + y = y * rot_x; + z = z * rot_x; + + // Reset the matrix to reflect the unwinding. + mat.set_row(0, x); + mat.set_row(1, y); + mat.set_row(2, z); + + // Return the three rotation components. + hpr[0] = heading; + hpr[1] = pitch; + hpr[2] = roll; +} + //////////////////////////////////////////////////////////////////// // Function: unwind_zup_rotation // Description: Extracts the rotation about the x, y, and z axes from @@ -169,7 +244,6 @@ unwind_zup_rotation(LMatrix3 &mat, LVecBase3 &hpr) { x = x * rot_y; y = y * rot_y; z = z * rot_y; - // Project the rotated X into the XY plane. LVector2 xy(x[0], x[1]); @@ -188,7 +262,6 @@ unwind_zup_rotation(LMatrix3 &mat, LVecBase3 &hpr) { y = y * rot_z; z = z * rot_z; - // Project the rotated Y into the YZ plane. LVector2 yz(y[1], y[2]); yz = normalize(yz); @@ -205,6 +278,80 @@ unwind_zup_rotation(LMatrix3 &mat, LVecBase3 &hpr) { y = y * rot_x; z = z * rot_x; + // Reset the matrix to reflect the unwinding. + mat.set_row(0, x); + mat.set_row(1, y); + mat.set_row(2, z); + + // Return the three rotation components. + hpr[0] = heading; + hpr[1] = pitch; + hpr[2] = roll; +} + +//////////////////////////////////////////////////////////////////// +// Function: unwind_zup_rotation +// Description: Extracts the rotation about the x, y, and z axes from +// the given hpr & scale matrix, given the indicated +// roll amount as a hint. Adjusts the matrix to +// eliminate the rotation. +// +// This function assumes the matrix is stored in a +// right-handed Z-up coordinate system. +//////////////////////////////////////////////////////////////////// +template +static void +unwind_zup_rotation(LMatrix3 &mat, LVecBase3 &hpr, + NumType roll) { + typedef LMatrix3 Matrix; + + // Extract the axes from the matrix. + LVector3 x, y, z; + x = mat.get_row(0); + y = mat.get_row(1); + z = mat.get_row(2); + + // Unwind the roll from the axes, and continue. + Matrix rot_y; + rot_y = Matrix::rotate_mat(roll, LVector3(0.0, 1.0, 0.0), + CS_zup_right); + + x = x * rot_y; + y = y * rot_y; + z = z * rot_y; + + // Project the rotated X into the XY plane. + LVector2 xy(x[0], x[1]); + xy = normalize(xy); + + // Compute the rotation about the +Z (up) axis. This is yaw, or + // "heading". + NumType heading = rad_2_deg(atan2(xy[1], xy[0])); + + // Unwind the heading, and continue. + Matrix rot_z; + rot_z = Matrix::rotate_mat(-heading, LVector3(0.0, 0.0, 1.0), + CS_zup_right); + + x = x * rot_z; + y = y * rot_z; + z = z * rot_z; + + // Project the rotated Y into the YZ plane. + LVector2 yz(y[1], y[2]); + yz = normalize(yz); + + // Compute the rotation about the +X (right) axis. This is pitch. + NumType pitch = rad_2_deg(atan2(yz[1], yz[0])); + + // Unwind the pitch. + Matrix rot_x; + rot_x = Matrix::rotate_mat(-pitch, LVector3(1.0, 0.0, 0.0), + CS_zup_right); + + x = x * rot_x; + y = y * rot_x; + z = z * rot_x; // Reset the matrix to reflect the unwinding. mat.set_row(0, x); @@ -308,6 +455,104 @@ _decompose_matrix(const LMatrix3 &mat, return !shear; } +//////////////////////////////////////////////////////////////////// +// Function: decompose_matrix +// Description: Extracts out the components of a 3x3 rotation matrix. +// Returns true if the scale and hpr completely describe +// the matrix, or false if there is also a shear +// component or if the matrix is not affine. +// +// This flavor of the function accepts an expected roll +// amount. This amount will be used as the roll +// component, rather than attempting to determine roll +// by examining the matrix; this helps alleviate roll +// instability due to roundoff errors or gimbal lock. +//////////////////////////////////////////////////////////////////// +template +static bool +_decompose_matrix(const LMatrix3 &mat, + LVecBase3 &scale, + LVecBase3 &hpr, + NumType roll, + CoordinateSystem cs) { + if (cs == CS_default) { + cs = default_coordinate_system; + } + + // Extract the rotation and scale, according to the coordinate + // system of choice. + bool shear; + + switch (cs) { + case CS_zup_right: + { + LMatrix3 rm(mat); + unwind_zup_rotation(rm, hpr, roll); + scale[0] = rm(0, 0); + scale[1] = rm(1, 1); + scale[2] = rm(2, 2); + shear = + (fabs(rm(0, 1)) + fabs(rm(0, 2)) + + fabs(rm(1, 0)) + fabs(rm(1, 2)) + + fabs(rm(2, 0)) + fabs(rm(2, 1))) >= 0.0001; + } + break; + + case CS_yup_right: + { + LMatrix3 rm(mat); + unwind_yup_rotation(rm, hpr, roll); + scale[0] = rm(0, 0); + scale[1] = rm(1, 1); + scale[2] = rm(2, 2); + shear = + (fabs(rm(0, 1)) + fabs(rm(0, 2)) + + fabs(rm(1, 0)) + fabs(rm(1, 2)) + + fabs(rm(2, 0)) + fabs(rm(2, 1))) >= 0.0001; + } + break; + + case CS_zup_left: + { + LMatrix3 lm(mat(0, 0), mat(0, 1), -mat(0, 2), + mat(1, 0), mat(1, 1), -mat(1, 2), + -mat(2, 0), -mat(2, 1), mat(2, 2)); + unwind_zup_rotation(lm, hpr, roll); + scale[0] = -lm(0, 0); + scale[1] = -lm(1, 1); + scale[2] = lm(2, 2); + shear = + (fabs(lm(0, 1)) + fabs(lm(0, 2)) + + fabs(lm(1, 0)) + fabs(lm(1, 2)) + + fabs(lm(2, 0)) + fabs(lm(2, 1))) >= 0.0001; + } + break; + + case CS_yup_left: + { + LMatrix3 lm(mat(0, 0), mat(0, 1), -mat(0, 2), + mat(1, 0), mat(1, 1), -mat(1, 2), + -mat(2, 0), -mat(2, 1), mat(2, 2)); + unwind_yup_rotation(lm, hpr, roll); + scale[0] = -lm(0, 0); + scale[1] = -lm(1, 1); + scale[2] = lm(2, 2); + shear = + (fabs(lm(0, 1)) + fabs(lm(0, 2)) + + fabs(lm(1, 0)) + fabs(lm(1, 2)) + + fabs(lm(2, 0)) + fabs(lm(2, 1))) >= 0.0001; + } + break; + + default: + linmath_cat.error() + << "Unexpected coordinate system!\n"; + return false; + } + + return !shear; +} + //////////////////////////////////////////////////////////////////// // Function: decompose_matrix // Description: Extracts out the components of an affine matrix. @@ -328,6 +573,33 @@ _decompose_matrix(const LMatrix4 &mat, return _decompose_matrix(mat.get_upper_3(), scale, hpr, cs); } +//////////////////////////////////////////////////////////////////// +// Function: decompose_matrix +// Description: Extracts out the components of an affine matrix. +// Returns true if the scale, hpr, translate +// completely describe the matrix, or false if there is +// also a shear component or if the matrix is not +// affine. +// +// This flavor of the function accepts an expected roll +// amount. This amount will be used as the roll +// component, rather than attempting to determine roll +// by examining the matrix; this helps alleviate roll +// instability due to roundoff errors or gimbal lock. +//////////////////////////////////////////////////////////////////// +template +INLINE bool +_decompose_matrix(const LMatrix4 &mat, + LVecBase3 &scale, + LVecBase3 &hpr, + LVecBase3 &translate, + NumType roll, + CoordinateSystem cs) { + // Get the translation first. + translate = mat.get_row3(3); + return _decompose_matrix(mat.get_upper_3(), scale, hpr, roll, cs); +} + void compose_matrix(LMatrix3f &mat, const LVecBase3f &scale, @@ -344,6 +616,15 @@ decompose_matrix(const LMatrix3f &mat, return _decompose_matrix(mat, scale, hpr, cs); } +bool +decompose_matrix(const LMatrix3f &mat, + LVecBase3f &scale, + LVecBase3f &hpr, + float roll, + CoordinateSystem cs) { + return _decompose_matrix(mat, scale, hpr, roll, cs); +} + void compose_matrix(LMatrix3d &mat, const LVecBase3d &scale, @@ -360,6 +641,15 @@ decompose_matrix(const LMatrix3d &mat, return _decompose_matrix(mat, scale, hpr, cs); } +bool +decompose_matrix(const LMatrix3d &mat, + LVecBase3d &scale, + LVecBase3d &hpr, + double roll, + CoordinateSystem cs) { + return _decompose_matrix(mat, scale, hpr, roll, cs); +} + void compose_matrix(LMatrix4f &mat, const LVecBase3f &scale, @@ -378,6 +668,16 @@ decompose_matrix(const LMatrix4f &mat, return _decompose_matrix(mat, scale, hpr, translate, cs); } +bool +decompose_matrix(const LMatrix4f &mat, + LVecBase3f &scale, + LVecBase3f &hpr, + LVecBase3f &translate, + float roll, + CoordinateSystem cs) { + return _decompose_matrix(mat, scale, hpr, translate, roll, cs); +} + void compose_matrix(LMatrix4d &mat, const LVecBase3d &scale, @@ -396,3 +696,13 @@ decompose_matrix(const LMatrix4d &mat, return _decompose_matrix(mat, scale, hpr, translate, cs); } +bool +decompose_matrix(const LMatrix4d &mat, + LVecBase3d &scale, + LVecBase3d &hpr, + LVecBase3d &translate, + double roll, + CoordinateSystem cs) { + return _decompose_matrix(mat, scale, hpr, translate, roll, cs); +} + diff --git a/panda/src/linmath/compose_matrix.h b/panda/src/linmath/compose_matrix.h index 0f67343d64..014e3499c8 100644 --- a/panda/src/linmath/compose_matrix.h +++ b/panda/src/linmath/compose_matrix.h @@ -42,6 +42,12 @@ decompose_matrix(const LMatrix3f &mat, LVecBase3f &scale, LVecBase3f &hpr, CoordinateSystem cs = CS_default); +EXPCL_PANDA bool +decompose_matrix(const LMatrix3f &mat, + LVecBase3f &scale, + LVecBase3f &hpr, + float roll, + CoordinateSystem cs = CS_default); EXPCL_PANDA void compose_matrix(LMatrix3d &mat, @@ -53,6 +59,12 @@ decompose_matrix(const LMatrix3d &mat, LVecBase3d &scale, LVecBase3d &hpr, CoordinateSystem cs = CS_default); +EXPCL_PANDA bool +decompose_matrix(const LMatrix3d &mat, + LVecBase3d &scale, + LVecBase3d &hpr, + double roll, + CoordinateSystem cs = CS_default); EXPCL_PANDA void compose_matrix(LMatrix4f &mat, @@ -69,6 +81,13 @@ decompose_matrix(const LMatrix4f &mat, LVecBase3f &hpr, LVecBase3f &translate, CoordinateSystem cs = CS_default); +EXPCL_PANDA bool +decompose_matrix(const LMatrix4f &mat, + LVecBase3f &scale, + LVecBase3f &hpr, + LVecBase3f &translate, + float roll, + CoordinateSystem cs = CS_default); INLINE bool decompose_matrix(const LMatrix4f &mat, float components[9], CoordinateSystem CS = CS_default); @@ -87,6 +106,13 @@ decompose_matrix(const LMatrix4d &mat, LVecBase3d &hpr, LVecBase3d &translate, CoordinateSystem cs = CS_default); +bool EXPCL_PANDA +decompose_matrix(const LMatrix4d &mat, + LVecBase3d &scale, + LVecBase3d &hpr, + LVecBase3d &translate, + double roll, + CoordinateSystem cs = CS_default); INLINE bool decompose_matrix(const LMatrix4d &mat, double components[9], CoordinateSystem cs = CS_default); diff --git a/panda/src/net/connectionReader.cxx b/panda/src/net/connectionReader.cxx index 020b3a2a25..39cf00eb4d 100644 --- a/panda/src/net/connectionReader.cxx +++ b/panda/src/net/connectionReader.cxx @@ -451,26 +451,17 @@ process_incoming_udp_data(SocketInfo *sinfo) { } DatagramUDPHeader header(buffer); - PRInt32 size = header.get_datagram_size(); PRInt8 *dp = buffer + datagram_udp_header_size; bytes_read -= datagram_udp_header_size; - PRInt32 datagram_bytes = min(bytes_read, size); - NetDatagram datagram(dp, datagram_bytes); + NetDatagram datagram(dp, bytes_read); if (_shutdown) { finish_socket(sinfo); return; } - if (bytes_read > datagram_bytes) { - // There were some extra bytes at the end of the datagram. Huh. - net_cat.error() - << "Discarding " << bytes_read - datagram_bytes - << " bytes following UDP datagram.\n"; - } - // Now that we've read all the data, it's time to finish the socket // so another thread can read the next datagram. finish_socket(sinfo); diff --git a/panda/src/net/datagramUDPHeader.cxx b/panda/src/net/datagramUDPHeader.cxx index 9589195169..9fa7efc1c5 100644 --- a/panda/src/net/datagramUDPHeader.cxx +++ b/panda/src/net/datagramUDPHeader.cxx @@ -19,16 +19,12 @@ DatagramUDPHeader:: DatagramUDPHeader(const NetDatagram &datagram) { const string &str = datagram.get_message(); - PRUint16 size = str.length(); - nassertv(size == str.length()); - PRUint16 checksum = 0; for (size_t p = 0; p < str.size(); p++) { checksum += (PRUint16)(PRUint8)str[p]; } // Now pack the header. - _header.add_uint16(size); _header.add_uint16(checksum); nassertv((int)_header.get_length() == datagram_udp_header_size); } @@ -44,18 +40,6 @@ DatagramUDPHeader:: DatagramUDPHeader(const void *data) : _header(data, datagram_udp_header_size) { } -//////////////////////////////////////////////////////////////////// -// Function: DatagramUDPHeader::get_datagram_size -// Access: Public -// Description: Returns the number of bytes in the associated -// datagram. -//////////////////////////////////////////////////////////////////// -int DatagramUDPHeader:: -get_datagram_size() const { - DatagramIterator di(_header); - return di.get_uint16(); -} - //////////////////////////////////////////////////////////////////// // Function: DatagramUDPHeader::get_datagram_checksum // Access: Public @@ -90,26 +74,19 @@ get_header() const { bool DatagramUDPHeader:: verify_datagram(const NetDatagram &datagram) const { const string &str = datagram.get_message(); - PRUint16 size = str.length(); - nassertr(size == str.length(), false); PRUint16 checksum = 0; for (size_t p = 0; p < str.size(); p++) { checksum += (PRUint16)(PRUint8)str[p]; } - if (size == get_datagram_size() && checksum == get_datagram_checksum()) { + if (checksum == get_datagram_checksum()) { return true; } if (net_cat.is_debug()) { net_cat.debug() << "Invalid datagram!\n"; - if (size != get_datagram_size()) { - net_cat.debug() - << " size is " << size << " bytes, header reports " - << get_datagram_size() << "\n"; - } if (checksum != get_datagram_checksum()) { net_cat.debug() << " checksum is " << checksum << ", header reports " diff --git a/panda/src/net/datagramUDPHeader.h b/panda/src/net/datagramUDPHeader.h index 440f000fca..e9a7e6d699 100644 --- a/panda/src/net/datagramUDPHeader.h +++ b/panda/src/net/datagramUDPHeader.h @@ -12,8 +12,7 @@ #include -static const int datagram_udp_header_size = - sizeof(PRUint16) + sizeof(PRUint16); +static const int datagram_udp_header_size = sizeof(PRUint16); class NetDatagram; @@ -30,7 +29,6 @@ public: DatagramUDPHeader(const NetDatagram &datagram); DatagramUDPHeader(const void *data); - int get_datagram_size() const; int get_datagram_checksum() const; const string &get_header() const; diff --git a/panda/src/tform/driveInterface.cxx b/panda/src/tform/driveInterface.cxx index a5b5389d9b..45b15e7de1 100644 --- a/panda/src/tform/driveInterface.cxx +++ b/panda/src/tform/driveInterface.cxx @@ -121,6 +121,8 @@ DriveInterface(const string &name) : DataNode(name) { _xyz.set(0.0, 0.0, 0.0); _hpr.set(0.0, 0.0, 0.0); _mat = LMatrix4f::ident_mat(); + _force_roll = 0.0; + _is_force_roll = true; _cs = default_coordinate_system; @@ -398,6 +400,7 @@ void DriveInterface:: reset() { _xyz.set(0.0, 0.0, 0.0); _hpr.set(0.0, 0.0, 0.0); + _force_roll = 0.0; _mat = LMatrix4f::ident_mat(); _up_arrow.clear(); _down_arrow.clear(); @@ -509,6 +512,10 @@ void DriveInterface:: set_hpr(float h, float p, float r) { _hpr.set(h, p, r); recompute(); + + if (_is_force_roll && r != _force_roll) { + reextract(); + } } void DriveInterface:: @@ -527,8 +534,58 @@ void DriveInterface:: set_r(float r) { _hpr[2] = r; recompute(); + + if (_is_force_roll && r != _force_roll) { + reextract(); + } } +//////////////////////////////////////////////////////////////////// +// Function: DriveInterface::set_force_roll +// Access: Public, Scheme +// Description: Sets the force_roll state. In this state, roll is +// always fixed to a particular value (typically zero), +// regardless of what it is explicitly set to via +// set_hpr(). +//////////////////////////////////////////////////////////////////// +void DriveInterface:: +set_force_roll(float force_roll) { + if (_is_force_roll) { + // If we already had force_roll() in effect, we have to + // recompensate for it. + if (_force_roll != force_roll) { + _force_roll = force_roll; + reextract(); + } + + } else { + _force_roll = force_roll; + _is_force_roll = true; + } +} + +//////////////////////////////////////////////////////////////////// +// Function: DriveInterface::is_force_roll +// Access: Public, Scheme +// Description: Returns true if the force_roll state is in effect, +// e.g. because of a previous call to set_force_roll(). +// In this state, the roll cannot be set to any value +// other than what the force_roll value indicates. +//////////////////////////////////////////////////////////////////// +bool DriveInterface:: +is_force_roll() const { + return _is_force_roll; +} + +//////////////////////////////////////////////////////////////////// +// Function: DriveInterface::clear_force_roll +// Access: Public, Scheme +// Description: Disables the force_roll state. See set_force_roll(). +//////////////////////////////////////////////////////////////////// +void DriveInterface:: +clear_force_roll() { + _is_force_roll = false; +} //////////////////////////////////////////////////////////////////// @@ -761,7 +818,19 @@ apply(double x, double y, bool any_button) { void DriveInterface:: reextract() { LVecBase3f scale; - decompose_matrix(_mat, scale, _hpr, _xyz); + + if (_is_force_roll) { + // We strongly discourage a roll other than _force_roll. + decompose_matrix(_mat, scale, _hpr, _xyz, _force_roll); + } else { + decompose_matrix(_mat, scale, _hpr, _xyz); + } + + if (tform_cat.is_debug()) { + tform_cat.debug() + << "Reextract " << _hpr << ", " << _xyz << " from:\n"; + _mat.write(tform_cat.debug(false), 2); + } } //////////////////////////////////////////////////////////////////// diff --git a/panda/src/tform/driveInterface.h b/panda/src/tform/driveInterface.h index f8f8bb8596..f6a77e4484 100644 --- a/panda/src/tform/driveInterface.h +++ b/panda/src/tform/driveInterface.h @@ -79,6 +79,10 @@ PUBLISHED: void set_p(float p); void set_r(float r); + void set_force_roll(float force_roll); + bool is_force_roll() const; + void clear_force_roll(); + void set_coordinate_system(CoordinateSystem cs); CoordinateSystem get_coordinate_system() const; @@ -114,6 +118,8 @@ private: LPoint3f _xyz; LVecBase3f _hpr; LMatrix4f _mat; + float _force_roll; + bool _is_force_roll; CoordinateSystem _cs; // Remember which arrow keys are being held down and which aren't,