TheCloud/panda3d
1
0
Fork 0
mirror of https://github.com/panda3d/panda3d.git synced 2025-10-03 18:31:55 -04:00
Code Issues Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
David Rose 2000-12-19 19:06:22 +00:00
parent 09be56009d
commit a5232c2b04
8 changed files with 432 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
panda/src/collide/collisionHandlerFloor.cxx
View File

@ -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";
}
}
}
}
}

314
panda/src/linmath/compose_matrix.cxx
View File

@ -125,6 +125,81 @@ unwind_yup_rotation(LMatrix3<NumType> &mat, LVecBase3<NumType> &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<class NumType>
static void
unwind_yup_rotation(LMatrix3<NumType> &mat, LVecBase3<NumType> &hpr,
NumType roll) {
typedef LMatrix3<NumType> Matrix;
// Extract the axes from the matrix.
LVector3<NumType> 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<NumType>(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<NumType> 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<NumType>(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<NumType> 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<NumType>(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
@ -170,6 +245,80 @@ unwind_zup_rotation(LMatrix3<NumType> &mat, LVecBase3<NumType> &hpr) {
y = y * rot_y;
z = z * rot_y;
// Project the rotated X into the XY plane.
LVector2<NumType> 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<NumType>(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<NumType> 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<NumType>(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);
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<class NumType>
static void
unwind_zup_rotation(LMatrix3<NumType> &mat, LVecBase3<NumType> &hpr,
NumType roll) {
typedef LMatrix3<NumType> Matrix;
// Extract the axes from the matrix.
LVector3<NumType> 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<NumType>(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<NumType> xy(x[0], x[1]);
@ -188,7 +337,6 @@ unwind_zup_rotation(LMatrix3<NumType> &mat, LVecBase3<NumType> &hpr) {
y = y * rot_z;
z = z * rot_z;
// Project the rotated Y into the YZ plane.
LVector2<NumType> yz(y[1], y[2]);
yz = normalize(yz);
@ -205,7 +353,6 @@ unwind_zup_rotation(LMatrix3<NumType> &mat, LVecBase3<NumType> &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);
@ -308,6 +455,104 @@ _decompose_matrix(const LMatrix3<NumType> &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<class NumType>
static bool
_decompose_matrix(const LMatrix3<NumType> &mat,
LVecBase3<NumType> &scale,
LVecBase3<NumType> &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<NumType> 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<NumType> 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<NumType> 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<NumType> 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<NumType> &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<class NumType>
INLINE bool
_decompose_matrix(const LMatrix4<NumType> &mat,
LVecBase3<NumType> &scale,
LVecBase3<NumType> &hpr,
LVecBase3<NumType> &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);
}

26
panda/src/linmath/compose_matrix.h
View File

@ -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);

11
panda/src/net/connectionReader.cxx
View File

@ -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);

25
panda/src/net/datagramUDPHeader.cxx
View File

@ -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 "

4
panda/src/net/datagramUDPHeader.h
View File

@ -12,8 +12,7 @@
#include <prtypes.h>
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;

71
panda/src/tform/driveInterface.cxx
View File

@ -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);
}
}
////////////////////////////////////////////////////////////////////

6
panda/src/tform/driveInterface.h
View File

@ -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,