diff --git a/choreolib/py/choreo/test/resources/swerve_test.traj b/choreolib/py/choreo/test/resources/swerve_test.traj index a9952922ee..05503e29df 100644 --- a/choreolib/py/choreo/test/resources/swerve_test.traj +++ b/choreolib/py/choreo/test/resources/swerve_test.traj @@ -8,7 +8,7 @@ "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.54, "h":8.21}}, "enabled":true}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":8.27, "y":4.105, "w":16.54, "h":8.21, "rotation":0.0}}, "enabled":true}], "targetDt":0.05 }, "params":{ @@ -18,7 +18,7 @@ "constraints":[ {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true}, - {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.54 m", "val":16.54}, "h":{"exp":"8.21 m", "val":8.21}}}, "enabled":true}], + {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"8.27 m", "val":8.27}, "y":{"exp":"4.105 m", "val":4.105}, "w":{"exp":"16.54 m", "val":16.54}, "h":{"exp":"8.21 m", "val":8.21}, "rotation":{"exp":"0 deg", "val":0.0}}}, "enabled":true}], "targetDt":{ "exp":"0.05 s", "val":0.05 diff --git a/src-core/src/generation/transformers/constraints.rs b/src-core/src/generation/transformers/constraints.rs index 12524504aa..101f5d6de9 100644 --- a/src-core/src/generation/transformers/constraints.rs +++ b/src-core/src/generation/transformers/constraints.rs @@ -15,6 +15,36 @@ fn fix_scope(idx: usize, removed_idxs: &[usize]) -> usize { idx - to_subtract } +/// Rotates a point around another point in 2D space. +/// +/// ```text +/// [x_new] [rot.cos, -rot.sin][x - other.x] [other.x] +/// [y_new] = [rot.sin, rot.cos][y - other.y] + [other.y] +/// ``` +/// +/// # Arguments +/// * `point` - The point to rotate as (x, y) +/// * `center` - The center point to rotate around as (x, y) +/// * `rotation` - The rotation angle in radians +/// +/// # Returns +/// The new rotated point as (x, y) +fn rotate_around(point: (f64, f64), center: (f64, f64), rotation: f64) -> (f64, f64) { + let cos_r = rotation.cos(); + let sin_r = rotation.sin(); + + // Translate to origin (center of rotation) + let rel_x = point.0 - center.0; + let rel_y = point.1 - center.1; + + // Apply rotation + let rotated_x = rel_x * cos_r - rel_y * sin_r; + let rotated_y = rel_x * sin_r + rel_y * cos_r; + + // Translate back + (center.0 + rotated_x, center.1 + rotated_y) +} + pub struct ConstraintSetter { guess_points: Vec, constraint_idx: Vec>, @@ -131,9 +161,27 @@ impl SwerveGenerationTransformer for ConstraintSetter { None => generator.wpt_keep_in_circle(from, x, y, r), Some(to) => generator.sgmt_keep_in_circle(from, to, x, y, r), }, - ConstraintData::KeepInRectangle { x, y, w, h } => { - let xs = vec![x, x + w, x + w, x]; - let ys = vec![y, y, y + h, y + h]; + ConstraintData::KeepInRectangle { x, y, w, h, rotation } => { + // x, y now represent the center of the rectangle + let center = (x, y); + + // Original corner points relative to center + let corners = vec![ + (x - w / 2.0, y - h / 2.0), // bottom-left + (x + w / 2.0, y - h / 2.0), // bottom-right + (x + w / 2.0, y + h / 2.0), // top-right + (x - w / 2.0, y + h / 2.0), // top-left + ]; + + let mut xs = Vec::new(); + let mut ys = Vec::new(); + + for corner in corners { + let (rotated_x, rotated_y) = rotate_around(corner, center, rotation); + xs.push(rotated_x); + ys.push(rotated_y); + } + match to_opt { None => generator.wpt_keep_in_polygon(from, xs, ys), Some(to) => generator.sgmt_keep_in_polygon(from, to, xs, ys), @@ -209,9 +257,27 @@ impl DifferentialGenerationTransformer for ConstraintSetter { None => generator.wpt_keep_in_circle(from, x, y, r), Some(to) => generator.sgmt_keep_in_circle(from, to, x, y, r), }, - ConstraintData::KeepInRectangle { x, y, w, h } => { - let xs = vec![x, x + w, x + w, x]; - let ys = vec![y, y, y + h, y + h]; + ConstraintData::KeepInRectangle { x, y, w, h, rotation } => { + // x, y now represent the center of the rectangle + let center = (x, y); + + // Original corner points relative to center + let corners = vec![ + (x - w / 2.0, y - h / 2.0), // bottom-left + (x + w / 2.0, y - h / 2.0), // bottom-right + (x + w / 2.0, y + h / 2.0), // top-right + (x - w / 2.0, y + h / 2.0), // top-left + ]; + + let mut xs = Vec::new(); + let mut ys = Vec::new(); + + for corner in corners { + let (rotated_x, rotated_y) = rotate_around(corner, center, rotation); + xs.push(rotated_x); + ys.push(rotated_y); + } + match to_opt { None => generator.wpt_keep_in_polygon(from, xs, ys), Some(to) => generator.sgmt_keep_in_polygon(from, to, xs, ys), diff --git a/src-core/src/spec/traj_schema_version.rs b/src-core/src/spec/traj_schema_version.rs index 576b4d75bb..455c5de2e1 100644 --- a/src-core/src/spec/traj_schema_version.rs +++ b/src-core/src/spec/traj_schema_version.rs @@ -1,2 +1,2 @@ -// Auto-generated by update_traj_schema.py -pub const TRAJ_SCHEMA_VERSION: u32 = 2; +// Auto-generated by update_traj_schema.py +pub const TRAJ_SCHEMA_VERSION: u32 = 2; diff --git a/src-core/src/spec/trajectory.rs b/src-core/src/spec/trajectory.rs index 5dee65e722..665aecc235 100644 --- a/src-core/src/spec/trajectory.rs +++ b/src-core/src/spec/trajectory.rs @@ -146,7 +146,7 @@ pub enum ConstraintData { /// A constraint to contain the bumpers within a circlular region of the field KeepInCircle { x: T, y: T, r: T }, /// A constraint to contain the bumpers within a rectangular region of the field - KeepInRectangle { x: T, y: T, w: T, h: T }, + KeepInRectangle { x: T, y: T, w: T, h: T, rotation: T }, /// A constraint to contain the bumpers within two line KeepInLane { tolerance: T }, /// A constraint to contain the bumpers outside a circlular region of the field @@ -192,11 +192,18 @@ impl ConstraintData { y: y.snapshot(), r: r.snapshot(), }, - ConstraintData::KeepInRectangle { x, y, w, h } => ConstraintData::KeepInRectangle { + ConstraintData::KeepInRectangle { + x, + y, + w, + h, + rotation, + } => ConstraintData::KeepInRectangle { x: x.snapshot(), y: y.snapshot(), w: w.snapshot(), h: h.snapshot(), + rotation: rotation.snapshot(), }, ConstraintData::KeepInLane { tolerance } => ConstraintData::KeepInLane { tolerance: tolerance.snapshot(), diff --git a/src/components/field/svg/constraintDisplay/KeepInRectangleOverlay.tsx b/src/components/field/svg/constraintDisplay/KeepInRectangleOverlay.tsx index a3bb7fa58a..9cb09962da 100644 --- a/src/components/field/svg/constraintDisplay/KeepInRectangleOverlay.tsx +++ b/src/components/field/svg/constraintDisplay/KeepInRectangleOverlay.tsx @@ -23,6 +23,8 @@ class KeepInRectangleOverlay extends Component< object > { rootRef: React.RefObject = React.createRef(); + private initialRotation: number = 0; + private initialMouseAngle: number = 0; componentDidMount() { if (this.rootRef.current) { // Theres probably a better way to do this @@ -100,17 +102,165 @@ class KeepInRectangleOverlay extends Component< d3.select( `#dragTarget-keepInRectangleRegion` ).call(dragHandleRegion); + + const dragHandleRotation = d3 + .drag() + .on("drag", (event) => this.dragRotation(event)) + .on("start", (event) => { + doc.history.startGroup(() => {}); + this.startRotation(event); + }) + .on("end", (_event) => { + doc.history.stopGroup(); + }) + .container(this.rootRef.current); + d3.select( + `#dragTarget-keepInRectangleRotation` + ).call(dragHandleRotation); } } dragPointTranslate(event: any, xOffset: boolean, yOffset: boolean) { const data = this.props.data; - console.log(xOffset, yOffset); - data.x.set(data.serialize.props.x.val + event.dx * (xOffset ? 0.0 : 1.0)); - data.y.set(data.serialize.props.y.val + event.dy * (yOffset ? 0.0 : 1.0)); + const rotation = data.serialize.props.rotation.val; + const centerX = data.serialize.props.x.val; // x,y are now center coordinates + const centerY = data.serialize.props.y.val; + const w = data.serialize.props.w.val; + const h = data.serialize.props.h.val; + + const center: [number, number] = [centerX, centerY]; + + // Calculate current rotated corners in world coordinates + const corners: [number, number][] = [ + [centerX - w / 2, centerY - h / 2], // bottom-left (index 0) + [centerX + w / 2, centerY - h / 2], // bottom-right (index 1) + [centerX + w / 2, centerY + h / 2], // top-right (index 2) + [centerX - w / 2, centerY + h / 2] // top-left (index 3) + ]; + const rotatedCorners = corners.map((corner) => + this.rotate_around(corner, center, rotation) + ); + + // Determine which corner should stay fixed + let fixedCornerIndex: number; + + if (!xOffset && !yOffset) { + // bottom-left corner drag + fixedCornerIndex = 2; // top-right stays fixed + } else if (xOffset && !yOffset) { + // bottom-right corner drag + fixedCornerIndex = 3; // top-left stays fixed + } else if (xOffset && yOffset) { + // top-right corner drag + fixedCornerIndex = 0; // bottom-left stays fixed + } else { + // top-left corner drag + fixedCornerIndex = 1; // bottom-right stays fixed + } + + // Position the dragged corner at the absolute cursor position + const newDraggedCorner: [number, number] = [event.x, event.y]; + + // Fixed corner stays in place + const fixedCorner = rotatedCorners[fixedCornerIndex]; + + // Calculate new center and dimensions from the diagonal corners + const newCenterX = (newDraggedCorner[0] + fixedCorner[0]) / 2; + const newCenterY = (newDraggedCorner[1] + fixedCorner[1]) / 2; + + // Calculate dimensions by transforming corners to the rectangle's local coordinate system + const cos_r = Math.cos(-rotation); + const sin_r = Math.sin(-rotation); + + // Transform both corners to local coordinates relative to new center + const draggedRelX = newDraggedCorner[0] - newCenterX; + const draggedRelY = newDraggedCorner[1] - newCenterY; + const fixedRelX = fixedCorner[0] - newCenterX; + const fixedRelY = fixedCorner[1] - newCenterY; - data.w.set(data.serialize.props.w.val - event.dx * (xOffset ? -1.0 : 1.0)); - data.h.set(data.serialize.props.h.val - event.dy * (yOffset ? -1.0 : 1.0)); + const draggedLocalX = draggedRelX * cos_r - draggedRelY * sin_r; + const draggedLocalY = draggedRelX * sin_r + draggedRelY * cos_r; + const fixedLocalX = fixedRelX * cos_r - fixedRelY * sin_r; + const fixedLocalY = fixedRelX * sin_r + fixedRelY * cos_r; + + // Calculate new width and height + const newW = Math.abs(draggedLocalX - fixedLocalX); + const newH = Math.abs(draggedLocalY - fixedLocalY); + + // Get robot dimensions (with bumpers) as minimum size + const minWidth = doc.robotConfig.bumper.length; + const minHeight = doc.robotConfig.bumper.width; + + // Apply minimum size constraints + const constrainedW = Math.max(newW, minWidth); + const constrainedH = Math.max(newH, minHeight); + + // If dimensions were constrained, recalculate center to keep fixed corner in place + let finalCenterX = newCenterX; + let finalCenterY = newCenterY; + + if (constrainedW !== newW || constrainedH !== newH) { + // When dimensions are constrained, keep the fixed corner in place + // and calculate the center position based on the constrained dimensions + + // Determine the local coordinates for the fixed corner in the constrained rectangle + const constrainedFixedLocalX = + ((fixedLocalX >= 0 ? 1 : -1) * constrainedW) / 2; + const constrainedFixedLocalY = + ((fixedLocalY >= 0 ? 1 : -1) * constrainedH) / 2; + + // Calculate where the center should be to keep the fixed corner in place + // The center is at a local offset from the fixed corner + const localCenterOffset: [number, number] = [ + -constrainedFixedLocalX, + -constrainedFixedLocalY + ]; + + // Rotate this offset by the rectangle's rotation to get world coordinates + const worldCenterOffset = this.rotate_around( + localCenterOffset, + [0, 0], + rotation + ); + + // Calculate the final center position + finalCenterX = fixedCorner[0] + worldCenterOffset[0]; + finalCenterY = fixedCorner[1] + worldCenterOffset[1]; + } + + // Calculate all new corner positions with the proposed center and dimensions + const newCorners: [number, number][] = [ + [finalCenterX - constrainedW / 2, finalCenterY - constrainedH / 2], // bottom-left + [finalCenterX + constrainedW / 2, finalCenterY - constrainedH / 2], // bottom-right + [finalCenterX + constrainedW / 2, finalCenterY + constrainedH / 2], // top-right + [finalCenterX - constrainedW / 2, finalCenterY + constrainedH / 2] // top-left + ]; + + // Rotate all corners to world coordinates + const newRotatedCorners = newCorners.map((corner) => + this.rotate_around(corner, [finalCenterX, finalCenterY], rotation) + ); + + // Check if any corner (except the fixed corner itself) is nearly at the same spot as the fixed corner + const hasCornerCollision = newRotatedCorners.some((corner, index) => { + // Skip the fixed corner itself + if (index === fixedCornerIndex) return false; + + const distance = Math.hypot( + corner[0] - fixedCorner[0], + corner[1] - fixedCorner[1] + ); + return distance < 0.1; // tolerance for corner collision + }); + + // Only update if no corner would collapse to the fixed corner position + if (!hasCornerCollision) { + // Update rectangle parameters (center-based) + data.x.set(finalCenterX); + data.y.set(finalCenterY); + data.w.set(constrainedW); + data.h.set(constrainedH); + } } dragRegionTranslate(event: any) { @@ -120,88 +270,174 @@ class KeepInRectangleOverlay extends Component< data.y.set(data.serialize.props.y.val + event.dy); } + startRotation(event: any) { + const data = this.props.data; + this.initialRotation = data.serialize.props.rotation.val; + + const centerX = data.serialize.props.x.val; // x,y are now center coordinates + const centerY = data.serialize.props.y.val; + + // Store initial mouse angle relative to center + const mouseX = event.x - centerX; + const mouseY = event.y - centerY; + this.initialMouseAngle = Math.atan2(mouseY, mouseX); + } + + dragRotation(event: any) { + const data = this.props.data; + const centerX = data.serialize.props.x.val; // x,y are now center coordinates + const centerY = data.serialize.props.y.val; + + // Get current mouse position relative to center + const mouseX = event.x - centerX; + const mouseY = event.y - centerY; + + // Calculate current mouse angle + const currentMouseAngle = Math.atan2(mouseY, mouseX); + + // Calculate the change in angle from initial mouse position + const angleDelta = currentMouseAngle - this.initialMouseAngle; + + // Set the rotation as initial rotation plus the change + data.rotation.set(this.initialRotation + angleDelta); + } + fixWidthHeight() { - if (this.props.data.serialize.props.w.val < 0.0) { - this.props.data.x.set( - this.props.data.serialize.props.x.val + - this.props.data.serialize.props.w.val - ); - this.props.data.w.set(-this.props.data.serialize.props.w.val); - } + // Get robot dimensions (with bumpers) as minimum size + const minWidth = doc.robotConfig.bumper.length; + const minHeight = doc.robotConfig.bumper.width; - if (this.props.data.serialize.props.h.val < 0.0) { - this.props.data.y.set( - this.props.data.serialize.props.y.val + - this.props.data.serialize.props.h.val - ); - this.props.data.h.set(-this.props.data.serialize.props.h.val); + // Ensure width and height are positive and meet minimum requirements + let width = this.props.data.serialize.props.w.val; + let height = this.props.data.serialize.props.h.val; + + if (width < 0.0) { + width = -width; + } + if (height < 0.0) { + height = -height; } + + // Apply minimum size constraints + width = Math.max(width, minWidth); + height = Math.max(height, minHeight); + + this.props.data.w.set(width); + this.props.data.h.set(height); + } + + rotate_around( + point: [number, number], + center: [number, number], + angle: number + ): [number, number] { + const cos_r = Math.cos(angle); + const sin_r = Math.sin(angle); + + const rel_x = point[0] - center[0]; + const rel_y = point[1] - center[1]; + + const rotated_x = rel_x * cos_r - rel_y * sin_r; + const rotated_y = rel_x * sin_r + rel_y * cos_r; + + return [center[0] + rotated_x, center[1] + rotated_y]; } render() { const data = this.props.data.serialize as DataMap["KeepInRectangle"]; - const x = data.props.x.val; - const y = data.props.y.val; + const centerX = data.props.x.val; // x,y now represent center + const centerY = data.props.y.val; const w = data.props.w.val; const h = data.props.h.val; + const rotation = data.props.rotation.val; + + const center: [number, number] = [centerX, centerY]; + + // Original corner points relative to center + const corners: [number, number][] = [ + [centerX - w / 2, centerY - h / 2], // bottom-left + [centerX + w / 2, centerY - h / 2], // bottom-right + [centerX + w / 2, centerY + h / 2], // top-right + [centerX - w / 2, centerY + h / 2] // top-left + ]; + + // Apply rotation around center using rotate_around method + const rotatedCorners = corners.map((corner) => + this.rotate_around(corner, center, rotation) + ); + + // Create SVG polygon path + const polygonPoints = rotatedCorners + .map((corner) => corner.join(",")) + .join(" "); + return ( - {/* Fill Rect*/} - = 0 ? x : x + w} - y={h >= 0 ? y : y + h} - width={Math.abs(w)} - height={Math.abs(h)} + {/* Fill Polygon*/} + - {/*Border Rect*/} - = 0 ? x : x + w} - y={h >= 0 ? y : y + h} - width={Math.abs(w)} - height={Math.abs(h)} + /> + {/*Border Polygon*/} + - {/* Corners */} - - - - + /> + {/* Rotated Corners */} + {rotatedCorners.map((corner, index) => ( + + ))} + {/* Rotation Handle - triangle at center of top edge */} + {(() => { + // Calculate center of top edge + const topLeftCorner = rotatedCorners[3]; + const topRightCorner = rotatedCorners[2]; + const topEdgeCenterX = (topLeftCorner[0] + topRightCorner[0]) / 2; + const topEdgeCenterY = (topLeftCorner[1] + topRightCorner[1]) / 2; + + // Triangle dimensions (matching waypoint style) + const triangleSize = DOT * 3; + const triangleHeight = triangleSize * 0.866; // √3/2 for equilateral triangle + + // Calculate angle for the triangle rotation (perpendicular to edge) + const edgeVectorX = topRightCorner[0] - topLeftCorner[0]; + const edgeVectorY = topRightCorner[1] - topLeftCorner[1]; + const edgeAngle = Math.atan2(edgeVectorY, edgeVectorX); + const triangleAngle = edgeAngle + Math.PI / 2; // perpendicular to edge + + return ( + + ); + })()} ); } diff --git a/src/document/ConstraintDefinitions.tsx b/src/document/ConstraintDefinitions.tsx index 134b3ea97f..e017dccf25 100644 --- a/src/document/ConstraintDefinitions.tsx +++ b/src/document/ConstraintDefinitions.tsx @@ -66,6 +66,7 @@ export type ConstraintDataTypeMap = { y: Expr; w: Expr; h: Expr; + rotation: Expr; }; KeepInLane: { tolerance: Expr; @@ -221,15 +222,13 @@ export const ConstraintDefinitions: defs = { properties: { x: { name: "X", - description: - "The x coordinate of the bottom left of the keep-in region", + description: "The x coordinate of the center of the keep-in region", dimension: Dimensions.Length, defaultVal: { exp: "0 m", val: 0 } }, y: { name: "Y", - description: - "The y coordinate of the bottom left of the keep-in region", + description: "The y coordinate of the center of the keep-in region", dimension: Dimensions.Length, defaultVal: { exp: "0 m", val: 0 } }, @@ -244,6 +243,12 @@ export const ConstraintDefinitions: defs = { description: "The height of the keep-in region", dimension: Dimensions.Length, defaultVal: { exp: "1 m", val: 1 } + }, + rotation: { + name: "R", + description: "The rotation angle of the rectangle around its center", + dimension: Dimensions.Angle, + defaultVal: { exp: "0 deg", val: 0 } } }, wptScope: true, diff --git a/src/document/PathListStore.ts b/src/document/PathListStore.ts index f38ed586b0..9808b48bab 100644 --- a/src/document/PathListStore.ts +++ b/src/document/PathListStore.ts @@ -131,8 +131,14 @@ export const PathListStore = types "first", "last", { - x: { exp: "0 m", val: 0.0 }, - y: { exp: "0 m", val: 0.0 }, + x: { + exp: `${FieldDimensions.FIELD_LENGTH / 2} m`, + val: FieldDimensions.FIELD_LENGTH / 2 + }, + y: { + exp: `${FieldDimensions.FIELD_WIDTH / 2} m`, + val: FieldDimensions.FIELD_WIDTH / 2 + }, w: { exp: `${FieldDimensions.FIELD_LENGTH} m`, val: FieldDimensions.FIELD_LENGTH