593 lines
21 KiB
C
593 lines
21 KiB
C
/**
|
||
* Copyright (c) 2026 Dominic Masters
|
||
*
|
||
* This software is released under the MIT License.
|
||
* https://opensource.org/licenses/MIT
|
||
*/
|
||
|
||
#include "entity/entitymanager.h"
|
||
|
||
// Decompose localTransform into the PRS cache. Only called when PRS_DIRTY.
|
||
static void entityPositionEnsurePRS(entityposition_t *pos) {
|
||
if(!(pos->flags & ENTITY_POSITION_FLAG_PRS_DIRTY)) return;
|
||
entityPositionDecompose(pos);
|
||
pos->flags &= ~ENTITY_POSITION_FLAG_PRS_DIRTY;
|
||
}
|
||
|
||
// Rebuild localTransform from the PRS cache. Only rebuilds what changed.
|
||
static void entityPositionEnsureLocal(entityposition_t *pos) {
|
||
const uint8_t dirty = pos->flags & (
|
||
ENTITY_POSITION_FLAG_ROTATION_DIRTY | ENTITY_POSITION_FLAG_POSITION_DIRTY
|
||
);
|
||
if(!dirty) return;
|
||
|
||
if(dirty & ENTITY_POSITION_FLAG_ROTATION_DIRTY) {
|
||
// Rotation or scale changed: rebuild columns 0-2 analytically (XYZ euler order).
|
||
const float c0 = cosf(pos->rotation[0]), s0 = sinf(pos->rotation[0]);
|
||
const float c1 = cosf(pos->rotation[1]), s1 = sinf(pos->rotation[1]);
|
||
const float c2 = cosf(pos->rotation[2]), s2 = sinf(pos->rotation[2]);
|
||
const float s0s1 = s0 * s1;
|
||
const float c0s1 = c0 * s1;
|
||
|
||
pos->localTransform[0][0] = c1 * c2 * pos->scale[0];
|
||
pos->localTransform[0][1] = (c0 * s2 + s0s1 * c2) * pos->scale[0];
|
||
pos->localTransform[0][2] = (s0 * s2 - c0s1 * c2) * pos->scale[0];
|
||
pos->localTransform[0][3] = 0.0f;
|
||
|
||
pos->localTransform[1][0] = -c1 * s2 * pos->scale[1];
|
||
pos->localTransform[1][1] = (c0 * c2 - s0s1 * s2) * pos->scale[1];
|
||
pos->localTransform[1][2] = (s0 * c2 + c0s1 * s2) * pos->scale[1];
|
||
pos->localTransform[1][3] = 0.0f;
|
||
|
||
pos->localTransform[2][0] = s1 * pos->scale[2];
|
||
pos->localTransform[2][1] = -s0 * c1 * pos->scale[2];
|
||
pos->localTransform[2][2] = c0 * c1 * pos->scale[2];
|
||
pos->localTransform[2][3] = 0.0f;
|
||
}
|
||
|
||
if(dirty & ENTITY_POSITION_FLAG_POSITION_DIRTY) {
|
||
// Only position changed: update column 3 only (no trig needed).
|
||
pos->localTransform[3][0] = pos->position[0];
|
||
pos->localTransform[3][1] = pos->position[1];
|
||
pos->localTransform[3][2] = pos->position[2];
|
||
pos->localTransform[3][3] = 1.0f;
|
||
}
|
||
|
||
pos->flags &= ~(ENTITY_POSITION_FLAG_ROTATION_DIRTY | ENTITY_POSITION_FLAG_POSITION_DIRTY);
|
||
}
|
||
|
||
// Recompute worldTransform from the parent chain. Only called when WORLD_DIRTY.
|
||
static void entityPositionEnsureWorld(entityposition_t *pos) {
|
||
if(!(pos->flags & ENTITY_POSITION_FLAG_WORLD_DIRTY)) return;
|
||
entityPositionEnsureLocal(pos);
|
||
|
||
if(pos->parentEntityId != ENTITY_ID_INVALID) {
|
||
// Parented: world = parent.world × local. worldTransform must be written
|
||
// because children (and this node's getters) read it.
|
||
entityposition_t *parent = componentGetData(
|
||
pos->parentEntityId, pos->parentComponentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
entityPositionEnsureWorld(parent);
|
||
glm_mat4_mul(parent->worldTransform, pos->localTransform, pos->worldTransform);
|
||
} else if(pos->childCount > 0) {
|
||
// Parentless root with children: children need a valid worldTransform to
|
||
// multiply against, but world == local, so just copy.
|
||
glm_mat4_copy(pos->localTransform, pos->worldTransform);
|
||
}
|
||
// Parentless leaf: world == local. Getters read localTransform directly;
|
||
// no copy needed.
|
||
|
||
pos->flags &= ~ENTITY_POSITION_FLAG_WORLD_DIRTY;
|
||
}
|
||
|
||
void entityPositionMarkDirty(entityposition_t *pos) {
|
||
if(pos->flags & ENTITY_POSITION_FLAG_WORLD_DIRTY) return;
|
||
pos->flags |= ENTITY_POSITION_FLAG_WORLD_DIRTY;
|
||
for(uint8_t i = 0; i < pos->childCount; i++) {
|
||
entityposition_t *child = componentGetData(
|
||
pos->childEntityIds[i], pos->childComponentIds[i], COMPONENT_TYPE_POSITION
|
||
);
|
||
entityPositionMarkDirty(child);
|
||
}
|
||
}
|
||
|
||
void entityPositionInit(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId
|
||
) {
|
||
entityposition_t *pos = componentGetData(
|
||
entityId, componentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
|
||
pos->flags = 0;
|
||
pos->parentEntityId = ENTITY_ID_INVALID;
|
||
pos->parentComponentId = COMPONENT_ID_INVALID;
|
||
pos->childCount = 0;
|
||
glm_vec3_zero(pos->position);
|
||
glm_vec3_zero(pos->rotation);
|
||
glm_vec3_one(pos->scale);
|
||
glm_mat4_identity(pos->localTransform);
|
||
glm_mat4_identity(pos->worldTransform);
|
||
}
|
||
|
||
void entityPositionLookAt(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId,
|
||
vec3 eye,
|
||
vec3 target,
|
||
vec3 up
|
||
) {
|
||
entityposition_t *pos = componentGetData(
|
||
entityId, componentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
glm_lookat(eye, target, up, pos->localTransform);
|
||
// localTransform is now authoritative; PRS cache is stale.
|
||
pos->flags = (pos->flags | ENTITY_POSITION_FLAG_PRS_DIRTY)
|
||
& ~(ENTITY_POSITION_FLAG_ROTATION_DIRTY | ENTITY_POSITION_FLAG_POSITION_DIRTY);
|
||
entityPositionMarkDirty(pos);
|
||
}
|
||
|
||
void entityPositionGetTransform(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId,
|
||
mat4 dest
|
||
) {
|
||
entityposition_t *pos = componentGetData(
|
||
entityId, componentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
entityPositionEnsureWorld(pos);
|
||
glm_mat4_copy(
|
||
pos->parentEntityId == ENTITY_ID_INVALID ? pos->localTransform : pos->worldTransform,
|
||
dest
|
||
);
|
||
}
|
||
|
||
void entityPositionGetLocalTransform(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId,
|
||
mat4 dest
|
||
) {
|
||
entityposition_t *pos = componentGetData(
|
||
entityId, componentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
entityPositionEnsureLocal(pos);
|
||
glm_mat4_copy(pos->localTransform, dest);
|
||
}
|
||
|
||
void entityPositionGetLocalPosition(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId,
|
||
vec3 dest
|
||
) {
|
||
entityposition_t *pos = componentGetData(
|
||
entityId, componentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
entityPositionEnsurePRS(pos);
|
||
glm_vec3_copy(pos->position, dest);
|
||
}
|
||
|
||
void entityPositionGetWorldPosition(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId,
|
||
vec3 dest
|
||
) {
|
||
entityposition_t *pos = componentGetData(
|
||
entityId, componentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
if(pos->parentEntityId == ENTITY_ID_INVALID) {
|
||
entityPositionEnsurePRS(pos);
|
||
glm_vec3_copy(pos->position, dest);
|
||
return;
|
||
}
|
||
entityPositionEnsureWorld(pos);
|
||
dest[0] = pos->worldTransform[3][0];
|
||
dest[1] = pos->worldTransform[3][1];
|
||
dest[2] = pos->worldTransform[3][2];
|
||
}
|
||
|
||
void entityPositionSetWorldPosition(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId,
|
||
vec3 position
|
||
) {
|
||
entityposition_t *pos = componentGetData(
|
||
entityId, componentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
if(pos->parentEntityId == ENTITY_ID_INVALID) {
|
||
glm_vec3_copy(position, pos->position);
|
||
pos->flags = (pos->flags | ENTITY_POSITION_FLAG_POSITION_DIRTY)
|
||
& ~ENTITY_POSITION_FLAG_PRS_DIRTY;
|
||
entityPositionMarkDirty(pos);
|
||
return;
|
||
}
|
||
entityposition_t *parent = componentGetData(
|
||
pos->parentEntityId, pos->parentComponentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
entityPositionEnsureWorld(parent);
|
||
mat4 invParent;
|
||
glm_mat4_inv(parent->worldTransform, invParent);
|
||
vec3 localPos;
|
||
glm_mat4_mulv3(invParent, position, 1.0f, localPos);
|
||
glm_vec3_copy(localPos, pos->position);
|
||
pos->flags = (pos->flags | ENTITY_POSITION_FLAG_POSITION_DIRTY)
|
||
& ~ENTITY_POSITION_FLAG_PRS_DIRTY;
|
||
entityPositionMarkDirty(pos);
|
||
}
|
||
|
||
void entityPositionSetLocalPosition(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId,
|
||
vec3 position
|
||
) {
|
||
entityposition_t *pos = componentGetData(
|
||
entityId, componentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
glm_vec3_copy(position, pos->position);
|
||
pos->flags = (pos->flags | ENTITY_POSITION_FLAG_POSITION_DIRTY)
|
||
& ~ENTITY_POSITION_FLAG_PRS_DIRTY;
|
||
entityPositionMarkDirty(pos);
|
||
}
|
||
|
||
void entityPositionGetLocalRotation(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId,
|
||
vec3 dest
|
||
) {
|
||
entityposition_t *pos = componentGetData(
|
||
entityId, componentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
entityPositionEnsurePRS(pos);
|
||
glm_vec3_copy(pos->rotation, dest);
|
||
}
|
||
|
||
void entityPositionGetWorldRotation(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId,
|
||
vec3 dest
|
||
) {
|
||
entityposition_t *pos = componentGetData(
|
||
entityId, componentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
if(pos->parentEntityId == ENTITY_ID_INVALID) {
|
||
entityPositionEnsurePRS(pos);
|
||
glm_vec3_copy(pos->rotation, dest);
|
||
return;
|
||
}
|
||
entityPositionEnsureWorld(pos);
|
||
const float (*wt)[4] = pos->worldTransform;
|
||
const float sx = sqrtf(wt[0][0]*wt[0][0] + wt[0][1]*wt[0][1] + wt[0][2]*wt[0][2]);
|
||
const float sy = sqrtf(wt[1][0]*wt[1][0] + wt[1][1]*wt[1][1] + wt[1][2]*wt[1][2]);
|
||
const float sz = sqrtf(wt[2][0]*wt[2][0] + wt[2][1]*wt[2][1] + wt[2][2]*wt[2][2]);
|
||
const float r00 = sx > 0.0f ? wt[0][0]/sx : 0.0f;
|
||
const float r10 = sy > 0.0f ? wt[1][0]/sy : 0.0f;
|
||
const float r20 = sz > 0.0f ? wt[2][0]/sz : 0.0f;
|
||
const float r01 = sx > 0.0f ? wt[0][1]/sx : 0.0f;
|
||
const float r11 = sy > 0.0f ? wt[1][1]/sy : 0.0f;
|
||
const float r21 = sz > 0.0f ? wt[2][1]/sz : 0.0f;
|
||
const float r22 = sz > 0.0f ? wt[2][2]/sz : 0.0f;
|
||
const float sinBeta = glm_clamp(r20, -1.0f, 1.0f);
|
||
dest[1] = asinf(sinBeta);
|
||
const float cosBeta = cosf(dest[1]);
|
||
if(fabsf(cosBeta) > 1e-6f) {
|
||
dest[0] = atan2f(-r21, r22);
|
||
dest[2] = atan2f(-r10, r00);
|
||
} else {
|
||
dest[2] = 0.0f;
|
||
dest[0] = (sinBeta > 0.0f) ? atan2f(r01, r11) : -atan2f(r01, r11);
|
||
}
|
||
}
|
||
|
||
void entityPositionSetLocalRotation(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId,
|
||
vec3 rotation
|
||
) {
|
||
entityposition_t *pos = componentGetData(
|
||
entityId, componentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
glm_vec3_copy(rotation, pos->rotation);
|
||
pos->flags = (pos->flags | ENTITY_POSITION_FLAG_ROTATION_DIRTY)
|
||
& ~ENTITY_POSITION_FLAG_PRS_DIRTY;
|
||
entityPositionMarkDirty(pos);
|
||
}
|
||
|
||
void entityPositionSetWorldRotation(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId,
|
||
vec3 rotation
|
||
) {
|
||
entityposition_t *pos = componentGetData(
|
||
entityId, componentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
if(pos->parentEntityId == ENTITY_ID_INVALID) {
|
||
glm_vec3_copy(rotation, pos->rotation);
|
||
pos->flags = (pos->flags | ENTITY_POSITION_FLAG_ROTATION_DIRTY)
|
||
& ~ENTITY_POSITION_FLAG_PRS_DIRTY;
|
||
entityPositionMarkDirty(pos);
|
||
return;
|
||
}
|
||
entityposition_t *parent = componentGetData(
|
||
pos->parentEntityId, pos->parentComponentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
entityPositionEnsureWorld(parent);
|
||
|
||
// Build target world rotation matrix (unit scale) from XYZ euler.
|
||
const float c0 = cosf(rotation[0]), s0 = sinf(rotation[0]);
|
||
const float c1 = cosf(rotation[1]), s1 = sinf(rotation[1]);
|
||
const float c2 = cosf(rotation[2]), s2 = sinf(rotation[2]);
|
||
const float s0s1 = s0*s1, c0s1 = c0*s1;
|
||
// Named wr[col_stored][row_stored] matching cglm column-major layout.
|
||
const float wr00 = c1*c2, wr01 = c0*s2 + s0s1*c2, wr02 = s0*s2 - c0s1*c2;
|
||
const float wr10 = -c1*s2, wr11 = c0*c2 - s0s1*s2, wr12 = s0*c2 + c0s1*s2;
|
||
const float wr20 = s1, wr21 = -s0*c1, wr22 = c0*c1;
|
||
|
||
// Normalize parent world columns to extract pure rotation.
|
||
const float (*pt)[4] = parent->worldTransform;
|
||
const float psx = sqrtf(pt[0][0]*pt[0][0] + pt[0][1]*pt[0][1] + pt[0][2]*pt[0][2]);
|
||
const float psy = sqrtf(pt[1][0]*pt[1][0] + pt[1][1]*pt[1][1] + pt[1][2]*pt[1][2]);
|
||
const float psz = sqrtf(pt[2][0]*pt[2][0] + pt[2][1]*pt[2][1] + pt[2][2]*pt[2][2]);
|
||
const float pr00 = psx > 0.f ? pt[0][0]/psx : 0.f;
|
||
const float pr01 = psx > 0.f ? pt[0][1]/psx : 0.f;
|
||
const float pr02 = psx > 0.f ? pt[0][2]/psx : 0.f;
|
||
const float pr10 = psy > 0.f ? pt[1][0]/psy : 0.f;
|
||
const float pr11 = psy > 0.f ? pt[1][1]/psy : 0.f;
|
||
const float pr12 = psy > 0.f ? pt[1][2]/psy : 0.f;
|
||
const float pr20 = psz > 0.f ? pt[2][0]/psz : 0.f;
|
||
const float pr21 = psz > 0.f ? pt[2][1]/psz : 0.f;
|
||
const float pr22 = psz > 0.f ? pt[2][2]/psz : 0.f;
|
||
|
||
// local_R = parent_R^T * world_R (R^-1 == R^T for orthogonal matrices).
|
||
// Compute only the 7 entries of the local rotation matrix needed for XYZ
|
||
// euler extraction (stored column-major: [col][row] = math [row][col]).
|
||
// sinBeta = stored[2][0] = math[0][2]
|
||
// r21/r22 = stored[2][1..2] = math[1..2][2]
|
||
// r10/r00 = stored[1][0], stored[0][0] = math[0][1], math[0][0]
|
||
// gimbal = stored[0][1], stored[1][1] = math[1][0], math[1][1]
|
||
const float lr00 = pr00*wr00 + pr01*wr10 + pr02*wr20; // math[0][0]
|
||
const float lr10 = pr00*wr01 + pr01*wr11 + pr02*wr21; // math[0][1]
|
||
const float lr20 = pr00*wr02 + pr01*wr12 + pr02*wr22; // math[0][2] → sinBeta
|
||
const float lr01 = pr10*wr00 + pr11*wr10 + pr12*wr20; // math[1][0]
|
||
const float lr11 = pr10*wr01 + pr11*wr11 + pr12*wr21; // math[1][1]
|
||
const float lr21 = pr10*wr02 + pr11*wr12 + pr12*wr22; // math[1][2] → r21
|
||
const float lr22 = pr20*wr02 + pr21*wr12 + pr22*wr22; // math[2][2] → r22
|
||
|
||
const float sinBeta = glm_clamp(lr20, -1.0f, 1.0f);
|
||
pos->rotation[1] = asinf(sinBeta);
|
||
const float cosBeta = cosf(pos->rotation[1]);
|
||
if(fabsf(cosBeta) > 1e-6f) {
|
||
pos->rotation[0] = atan2f(-lr21, lr22);
|
||
pos->rotation[2] = atan2f(-lr10, lr00);
|
||
} else {
|
||
pos->rotation[2] = 0.0f;
|
||
pos->rotation[0] = (sinBeta > 0.0f) ? atan2f(lr01, lr11) : -atan2f(lr01, lr11);
|
||
}
|
||
pos->flags = (pos->flags | ENTITY_POSITION_FLAG_ROTATION_DIRTY)
|
||
& ~ENTITY_POSITION_FLAG_PRS_DIRTY;
|
||
entityPositionMarkDirty(pos);
|
||
}
|
||
|
||
void entityPositionGetLocalScale(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId,
|
||
vec3 dest
|
||
) {
|
||
entityposition_t *pos = componentGetData(
|
||
entityId, componentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
entityPositionEnsurePRS(pos);
|
||
glm_vec3_copy(pos->scale, dest);
|
||
}
|
||
|
||
void entityPositionGetWorldScale(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId,
|
||
vec3 dest
|
||
) {
|
||
entityposition_t *pos = componentGetData(
|
||
entityId, componentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
if(pos->parentEntityId == ENTITY_ID_INVALID) {
|
||
entityPositionEnsurePRS(pos);
|
||
glm_vec3_copy(pos->scale, dest);
|
||
return;
|
||
}
|
||
entityPositionEnsureWorld(pos);
|
||
const float (*wt)[4] = pos->worldTransform;
|
||
dest[0] = sqrtf(wt[0][0]*wt[0][0] + wt[0][1]*wt[0][1] + wt[0][2]*wt[0][2]);
|
||
dest[1] = sqrtf(wt[1][0]*wt[1][0] + wt[1][1]*wt[1][1] + wt[1][2]*wt[1][2]);
|
||
dest[2] = sqrtf(wt[2][0]*wt[2][0] + wt[2][1]*wt[2][1] + wt[2][2]*wt[2][2]);
|
||
}
|
||
|
||
void entityPositionSetLocalScale(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId,
|
||
vec3 scale
|
||
) {
|
||
entityposition_t *pos = componentGetData(
|
||
entityId, componentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
glm_vec3_copy(scale, pos->scale);
|
||
pos->flags = (pos->flags | ENTITY_POSITION_FLAG_ROTATION_DIRTY)
|
||
& ~ENTITY_POSITION_FLAG_PRS_DIRTY;
|
||
entityPositionMarkDirty(pos);
|
||
}
|
||
|
||
void entityPositionSetWorldScale(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId,
|
||
vec3 scale
|
||
) {
|
||
entityposition_t *pos = componentGetData(
|
||
entityId, componentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
if(pos->parentEntityId == ENTITY_ID_INVALID) {
|
||
glm_vec3_copy(scale, pos->scale);
|
||
pos->flags = (pos->flags | ENTITY_POSITION_FLAG_ROTATION_DIRTY)
|
||
& ~ENTITY_POSITION_FLAG_PRS_DIRTY;
|
||
entityPositionMarkDirty(pos);
|
||
return;
|
||
}
|
||
entityposition_t *parent = componentGetData(
|
||
pos->parentEntityId, pos->parentComponentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
entityPositionEnsureWorld(parent);
|
||
const float (*pt)[4] = parent->worldTransform;
|
||
const float psx = sqrtf(pt[0][0]*pt[0][0] + pt[0][1]*pt[0][1] + pt[0][2]*pt[0][2]);
|
||
const float psy = sqrtf(pt[1][0]*pt[1][0] + pt[1][1]*pt[1][1] + pt[1][2]*pt[1][2]);
|
||
const float psz = sqrtf(pt[2][0]*pt[2][0] + pt[2][1]*pt[2][1] + pt[2][2]*pt[2][2]);
|
||
pos->scale[0] = psx > 0.0f ? scale[0] / psx : scale[0];
|
||
pos->scale[1] = psy > 0.0f ? scale[1] / psy : scale[1];
|
||
pos->scale[2] = psz > 0.0f ? scale[2] / psz : scale[2];
|
||
pos->flags = (pos->flags | ENTITY_POSITION_FLAG_ROTATION_DIRTY)
|
||
& ~ENTITY_POSITION_FLAG_PRS_DIRTY;
|
||
entityPositionMarkDirty(pos);
|
||
}
|
||
|
||
void entityPositionSetParent(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId,
|
||
const entityid_t parentEntityId,
|
||
const componentid_t parentComponentId
|
||
) {
|
||
entityposition_t *pos = componentGetData(
|
||
entityId, componentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
|
||
// Remove from old parent's child list.
|
||
if(pos->parentEntityId != ENTITY_ID_INVALID) {
|
||
entityposition_t *oldParent = componentGetData(
|
||
pos->parentEntityId, pos->parentComponentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
for(uint8_t i = 0; i < oldParent->childCount; i++) {
|
||
if(
|
||
oldParent->childEntityIds[i] == entityId &&
|
||
oldParent->childComponentIds[i] == componentId
|
||
) {
|
||
oldParent->childCount--;
|
||
for(uint8_t j = i; j < oldParent->childCount; j++) {
|
||
oldParent->childEntityIds[j] = oldParent->childEntityIds[j + 1];
|
||
oldParent->childComponentIds[j] = oldParent->childComponentIds[j + 1];
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
pos->parentEntityId = parentEntityId;
|
||
pos->parentComponentId = parentComponentId;
|
||
|
||
// Register with new parent.
|
||
if(parentEntityId != ENTITY_ID_INVALID) {
|
||
entityposition_t *parent = componentGetData(
|
||
parentEntityId, parentComponentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
if(parent->childCount < ENTITY_POSITION_CHILDREN_MAX) {
|
||
parent->childEntityIds[parent->childCount] = entityId;
|
||
parent->childComponentIds[parent->childCount] = componentId;
|
||
parent->childCount++;
|
||
}
|
||
}
|
||
|
||
entityPositionMarkDirty(pos);
|
||
}
|
||
|
||
entityposition_t *entityPositionGet(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId
|
||
) {
|
||
return componentGetData(
|
||
entityId, componentId, COMPONENT_TYPE_POSITION
|
||
);
|
||
}
|
||
|
||
void entityPositionRebuild(entityposition_t *pos) {
|
||
pos->flags = (pos->flags | ENTITY_POSITION_FLAG_ROTATION_DIRTY | ENTITY_POSITION_FLAG_POSITION_DIRTY)
|
||
& ~ENTITY_POSITION_FLAG_PRS_DIRTY;
|
||
entityPositionMarkDirty(pos);
|
||
}
|
||
|
||
void entityPositionDisposeDeep(
|
||
const entityid_t entityId,
|
||
const componentid_t componentId
|
||
) {
|
||
entityposition_t *pos = entityPositionGet(entityId, componentId);
|
||
|
||
// Detach from parent so the parent's child list stays consistent.
|
||
if(pos->parentEntityId != ENTITY_ID_INVALID) {
|
||
entityPositionSetParent(entityId, componentId, ENTITY_ID_INVALID, COMPONENT_ID_INVALID);
|
||
}
|
||
|
||
// Copy the child list before disposing self (entityDispose invalidates pos).
|
||
uint8_t childCount = pos->childCount;
|
||
entityid_t childEntityIds[ENTITY_POSITION_CHILDREN_MAX];
|
||
componentid_t childComponentIds[ENTITY_POSITION_CHILDREN_MAX];
|
||
for(uint8_t i = 0; i < childCount; i++) {
|
||
childEntityIds[i] = pos->childEntityIds[i];
|
||
childComponentIds[i] = pos->childComponentIds[i];
|
||
// Sever the child's parent link so it won't try to modify our disposed data.
|
||
entityposition_t *child = entityPositionGet(childEntityIds[i], childComponentIds[i]);
|
||
child->parentEntityId = ENTITY_ID_INVALID;
|
||
child->parentComponentId = COMPONENT_ID_INVALID;
|
||
}
|
||
|
||
entityDispose(entityId);
|
||
|
||
for(uint8_t i = 0; i < childCount; i++) {
|
||
entityPositionDisposeDeep(childEntityIds[i], childComponentIds[i]);
|
||
}
|
||
}
|
||
|
||
void entityPositionDecompose(entityposition_t *pos) {
|
||
// Translation: column 3
|
||
pos->position[0] = pos->localTransform[3][0];
|
||
pos->position[1] = pos->localTransform[3][1];
|
||
pos->position[2] = pos->localTransform[3][2];
|
||
|
||
// Scale: length of each basis column (xyz only)
|
||
pos->scale[0] = sqrtf(
|
||
pos->localTransform[0][0] * pos->localTransform[0][0] +
|
||
pos->localTransform[0][1] * pos->localTransform[0][1] +
|
||
pos->localTransform[0][2] * pos->localTransform[0][2]
|
||
);
|
||
pos->scale[1] = sqrtf(
|
||
pos->localTransform[1][0] * pos->localTransform[1][0] +
|
||
pos->localTransform[1][1] * pos->localTransform[1][1] +
|
||
pos->localTransform[1][2] * pos->localTransform[1][2]
|
||
);
|
||
pos->scale[2] = sqrtf(
|
||
pos->localTransform[2][0] * pos->localTransform[2][0] +
|
||
pos->localTransform[2][1] * pos->localTransform[2][1] +
|
||
pos->localTransform[2][2] * pos->localTransform[2][2]
|
||
);
|
||
|
||
// Normalize columns to isolate the rotation matrix (9 floats, no mat4 needed).
|
||
const float invS0 = pos->scale[0] > 0.0f ? 1.0f / pos->scale[0] : 0.0f;
|
||
const float invS1 = pos->scale[1] > 0.0f ? 1.0f / pos->scale[1] : 0.0f;
|
||
const float invS2 = pos->scale[2] > 0.0f ? 1.0f / pos->scale[2] : 0.0f;
|
||
|
||
const float r00 = pos->localTransform[0][0] * invS0;
|
||
const float r01 = pos->localTransform[0][1] * invS0;
|
||
const float r02 = pos->localTransform[0][2] * invS0;
|
||
const float r10 = pos->localTransform[1][0] * invS1;
|
||
const float r11 = pos->localTransform[1][1] * invS1;
|
||
const float r20 = pos->localTransform[2][0] * invS2;
|
||
const float r21 = pos->localTransform[2][1] * invS2;
|
||
const float r22 = pos->localTransform[2][2] * invS2;
|
||
|
||
// Extract XYZ euler angles (R = Rx * Ry * Rz, column-major)
|
||
const float sinBeta = glm_clamp(r20, -1.0f, 1.0f);
|
||
pos->rotation[1] = asinf(sinBeta);
|
||
const float cosBeta = cosf(pos->rotation[1]);
|
||
|
||
if(fabsf(cosBeta) > 1e-6f) {
|
||
pos->rotation[0] = atan2f(-r21, r22);
|
||
pos->rotation[2] = atan2f(-r10, r00);
|
||
} else {
|
||
// Gimbal lock: pin Z to 0, recover X.
|
||
pos->rotation[2] = 0.0f;
|
||
pos->rotation[0] = (sinBeta > 0.0f)
|
||
? atan2f(r01, r11)
|
||
: -atan2f(r01, r11);
|
||
}
|
||
}
|